机器人操作系统（ROS）课件7.3多传感器数据同步

机器人操作系统（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