機(jī)器人操作系統(tǒng)（ROS）課件7.3多傳感器數(shù)據(jù)同步

機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）多傳感器數(shù)據(jù)同步7.3機(jī)器人操作系統(tǒng)（ROS）7.3.1message_filters程序庫(kù)介紹message_filters是一個(gè)用于roscpp和rospy的實(shí)用程序庫(kù)，類似于一個(gè)消息緩存器，當(dāng)消息到達(dá)消息過(guò)濾器時(shí)，可能并不會(huì)立即輸出，而是在稍后的時(shí)間點(diǎn)里滿足一定條件下輸出。message_filters庫(kù)中包含有TimeSynchronizer時(shí)間同步器，它接收來(lái)自多個(gè)源的不同類型的消息，并且僅當(dāng)它們?cè)诰哂邢嗤瑫r(shí)間戳的每個(gè)源上接收到消息時(shí)才輸出它們，也就是起到了一個(gè)消息同步輸出的效果。

對(duì)齊傳感器數(shù)據(jù)時(shí)間戳有兩種策略：

①時(shí)間戳完全對(duì)齊策略(ExactTimePolicy)

②時(shí)間戳相近策略(ApproximateTimePolicy)機(jī)器人操作系統(tǒng)（ROS）7.3.2多傳感器數(shù)據(jù)同步實(shí)驗(yàn)

本實(shí)驗(yàn)使用相機(jī)和IMU，時(shí)間同步示意圖如圖所示，分別定義IMU、相機(jī)兩個(gè)傳感器節(jié)點(diǎn)，分別發(fā)布/imu/angle，/usb_cam/image_raw話題，對(duì)應(yīng)的發(fā)布頻率分別為100Hz和30Hz。

通過(guò)TimeSynchronizer統(tǒng)一接收這兩個(gè)話題，當(dāng)兩個(gè)話題的時(shí)間戳對(duì)齊時(shí)，進(jìn)入callback回調(diào)函數(shù)。采用時(shí)間戳相近的對(duì)齊方式可通過(guò)設(shè)定一個(gè)時(shí)間參數(shù)，如0.1s，如果兩個(gè)話題的時(shí)間戳相差在0.1s之內(nèi)則認(rèn)為已經(jīng)對(duì)齊。

數(shù)據(jù)同步的頻率以頻率最低的話題為準(zhǔn)，即/usb_cam/image_raw的30Hz頻率?；赥imeSynchronizer的時(shí)間同步示意圖機(jī)器人操作系統(tǒng)（ROS）7.3.2多傳感器數(shù)據(jù)同步實(shí)驗(yàn)#!/usr/bin/envpython#-*-coding:utf-8-*-importrospyfromgeometry_msgs.msgimportPoseStampeddata_z=0.0defangle_publisher(): #ROS節(jié)點(diǎn)初始化

rospy.init_node('angle_publisher',anonymous=True) #發(fā)布話題，話題名為/imu/angle，類型為PoseStamped，隊(duì)列長(zhǎng)度為10 angle_pub=rospy.Publisher('/imu/angle',PoseStamped,queue_size=10) #設(shè)置循環(huán)的頻率 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 #發(fā)布消息

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

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

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(): #初始化節(jié)點(diǎn)

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) #如果兩個(gè)話題的時(shí)間戳相差在0.1s之內(nèi)則認(rèn)為時(shí)間戳已經(jīng)對(duì)齊

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()返回一個(gè)列表，包含有指定路徑下的目錄和文件的名稱

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

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

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)#清空（創(chuàng)建）txt withopen(join(path,'data.txt'),'a')asf:#打開(kāi)data.txt，如果該文件已存在，新的內(nèi)容將會(huì)被寫(xiě)入到已有內(nèi)容之后。如果該文件不存在，創(chuàng)建新文件進(jìn)行寫(xiě)入

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

f.truncate()#截?cái)辔募?/p>

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

運(yùn)行數(shù)據(jù)同步節(jié)點(diǎn)數(shù)據(jù)實(shí)現(xiàn)同步采集，圖片數(shù)據(jù)保存在Dataset/imgs