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

机器人操作系统（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#设置保存路径，nu