機器人操作系統（ROS）機器人操作系統（ROS）多傳感器數據同步7.3機器人操作系統（ROS）7.3.1message_filters程序庫介紹message_filters是一個用于roscpp和rospy的實用程序庫，類似于一個消息緩存器，當消息到達消息過濾器時，可能并不會立即輸出，而是在稍后的時間點里滿足一定條件下輸出。message_filters庫中包含有TimeSynchronizer時間同步器，它接收來自多個源的不同類型的消息，并且僅當它們在具有相同時間戳的每個源上接收到消息時才輸出它們，也就是起到了一個消息同步輸出的效果。

對齊傳感器數據時間戳有兩種策略：

①時間戳完全對齊策略(ExactTimePolicy)

②時間戳相近策略(ApproximateTimePolicy)機器人操作系統（ROS）7.3.2多傳感器數據同步實驗

本實驗使用相機和IMU，時間同步示意圖如圖所示，分別定義IMU、相機兩個傳感器節點，分別發布/imu/angle，/usb_cam/image_raw話題，對應的發布頻率分別為100Hz和30Hz。

通過TimeSynchronizer統一接收這兩個話題，當兩個話題的時間戳對齊時，進入callback回調函數。采用時間戳相近的對齊方式可通過設定一個時間參數，如0.1s，如果兩個話題的時間戳相差在0.1s之內則認為已經對齊。

數據同步的頻率以頻率最低的話題為準，即/usb_cam/image_raw的30Hz頻率。基于TimeSynchronizer的時間同步示意圖機器人操作系統（ROS）7.3.2多傳感器數據同步實驗#!/usr/bin/envpython#-*-coding:utf-8-*-importrospyfromgeometry_msgs.msgimportPoseStampeddata_z=0.0defangle_publisher(): #ROS節點初始化

rospy.init_node('angle_publisher',anonymous=True) #發布話題，話題名為/imu/angle，類型為PoseStamped，隊列長度為10 angle_pub=rospy.Publisher('/imu/angle',PoseStamped,queue_size=10) #設置循環的頻率 rate=rospy.Rate(100) whilenotrospy.is_shutdown(): globaldata_z angle=PoseStamped() angle.header.frame_id="angle" angle.header.stamp=rospy.Time.now() data_z+=0.01 angle.pose.position.z=data_z #發布消息

angle_pub.publish(angle) rospy.loginfo("Publshangle%0.2fdeg",angle.pose.position.z) #按照循環頻率延時

rate.sleep()if__name__=='__main__': try: angle_publisher() exceptrospy.ROSInterruptException: pass建立IMU發布節點(angle_pub.py)：angle_pub.py的功能是模擬IMU不停地發布角度信息。它發布/imu/angle話題消息，類型為geometry_msgs/PoseStamped。機器人操作系統（ROS）7.3.2多傳感器數據同步實驗#!/usr/bin/envpython#-*-coding:utf-8-*-importrospyimportcv2importnumpyasnpimportmessage_filtersimportosfromos.pathimportjoinfromsensor_msgs.msgimportImagefromgeometry_msgs.msgimportPoseStampedfromcv_bridgeimportCvBridge,CvBridgeError#初始化參數frame_id=0gps_id=0#設置保存路徑，nuaa-frl為本機用戶名，讀者需自行修改path="/home/nuaa-frl/Dataset/"img_path="/home/nuaa-frl/Dataset/imgs"bridge=CvBridge()flag=Trueglobalpath_filepath_file='/home/nuaa-frl/temp'建立數據同步節點(CAM_IMU_test.py)：CAM_IMU_test.py的功能是將相機和IMU的數據進行同步采集并保存。它訂閱/imu/angle話題和/usb_cam/image_raw話題，并通過時間同步器TimeSynchronizer進行數據同步采集，最終保存到本地文件中。機器人操作系統（ROS）7.3.2多傳感器數據同步實驗defcallback(angle,image): ifflag==True: rospy.loginfo("datarecieved!") globalframe_id globalbridge imu_yaw=angle.pose.position.z #創建data.txt，保存imu角度數據

data="{}{}\n".format(frame_id,imu_yaw) pose_file=open(join(path,'data.txt'),'a') pose_file.writelines(data) pose_file.close() #保存圖片

down_img=bridge.imgmsg_to_cv2(image) image_name=join(img_path,str(frame_id)+".jpg") cv2.imwrite(image_name,down_img) frame_id+=1 rospy.loginfo("datarecorded!")deflistener(): #初始化節點

rospy.init_node('listener',anonymous=True) time=rospy.Time().now() #訂閱/imu/angle話題

angle_sub=message_filters.Subscriber('/imu/angle',PoseStamped) #訂閱/usb_cam/image_raw話題

image_sub=message_filters.Subscriber('/usb_cam/image_raw',Image) #如果兩個話題的時間戳相差在0.1s之內則認為時間戳已經對齊

ts=message_filters.ApproximateTimeSynchronizer([angle_sub,image_sub],1000,0.1) ts.registerCallback(callback) rospy.loginfo("successfullyinitialized!") rospy.spin()if__name__=='__main__': #清空存放圖片的文件夾

foriinos.listdir(img_path):#os.listdir()返回一個列表，包含有指定路徑下的目錄和文件的名稱

path_file=os.path.join(img_path,i)#os.path.join()用于路徑拼接文件路徑

ifos.path.isfile(path_file):#os.path.isfile()判斷某一對象(需提供絕對路徑)是否為文件

os.remove(path_file)#os.remove()刪除指定路徑的文件

else: forfinos.listdir(path_file): path_file2=os.path.join(path_file,f) ifos.path.isfile(path_file2): os.remove(path_file2)#清空（創建）txt withopen(join(path,'data.txt'),'a')asf:#打開data.txt，如果該文件已存在，新的內容將會被寫入到已有內容之后。如果該文件不存在，創建新文件進行寫入

f.seek(0)#從文件開頭讀取文件

f.truncate()#截斷文件

listener() rospy.loginfo("datasuccessfullysaved!")機器人操作系統（ROS）7.3.2多傳感器數據同步實驗運行以下命令下載相機驅動程序(具體版本視安裝的ros版本決定)：$sudoapt-getinstallros-melodic-usb-cam接入相機，運行以下命令啟動相機節點：$roslaunchusb_camusb_cam-test.launch安裝相機驅動啟動相機節點機器人操作系統（ROS）7.3.2多傳感器數據同步實驗運行以下命令啟動IMU節點：$rosrunCAM_IMU_SYNCangle_pub.py運行如下命令查看相機和IMU節點發布頻率：$rostopichz/usb_cam/image_raw$rostopichz/imu/angle啟動IMU節點相機話題發布頻率IMU話題發布頻率機器人操作系統（ROS）7.3.2多傳感器數據同步實驗運行以下命令啟動數據同步節點:$rosrunCAM_IMU_SYNCCAM_IMU_test.py

運行數據同步節點數據實現同步采集，圖片數據保存在Dataset/imgs