[转]ROS中使用message_filters进行多传感器消息同步
转:http://www.rosclub.cn/post-1030.html
最近实验室老师在做一个多传感器数据采集实验,涉及到了消息同步。所以就学习了ROS官网下的消息同步工具message_filters。 http://wiki.ros.org/message_filters 消息同步有两种方式,暂且称之为松同步与紧同步,紧同步是精确的同步,松同步是粗略的同步。我使用的是C++下的松同步我的代码如下:#include <message_filters/subscriber.h> #include <message_filters/synchronizer.h> #include <message_filters/sync_policies/approximate_tim
最近实验室老师在做一个多传感器数据采集实验,涉及到了消息同步。所以就学习了ROS官网下的消息同步工具message_filters。
http://wiki.ros.org/message_filters
消息同步有两种方式,暂且称之为松同步与紧同步,紧同步是精确的同步,松同步是粗略的同步。我使用的是C++下的松同步我的代码如下:
#include <message_filters/subscriber.h> #include <message_filters/synchronizer.h> #include <message_filters/sync_policies/approximate_time.h> #include <sensor_msgs/Image.h> #include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/LaserScan.h> #include "image_transport/image_transport.h" #include "sensor_msgs/CompressedImage.h" #include "ros/ros.h" #include "sensor_msgs/Imu.h" #include "nav_msgs/Odometry.h" #include "sensor_msgs/CameraInfo.h" #include "rosbag/bag.h" #include "ctime" #include "time.h" /* ros::Publisher Velodyne_pub; ros::Publisher Hokuyo_pub; ros::Publisher Ominivision_pub; ros::Publisher Kinect2color_pub; ros::Publisher Kinect2depth_pub; ros::Publisher Imu_pub; ros::Publisher Odom_pub; ros::Publisher Kinect2camera_info_pub;*/ rosbag::Bag bag_record; using namespace std; string int2string(int value) { stringstream ss; ss<<value; return ss.str(); } void callback(const sensor_msgs:: PointCloud2ConstPtr& point_cloud2, const sensor_msgs::LaserScan::ConstPtr& laser_scan, const sensor_msgs::CompressedImageConstPtr& ominivision_msg, const sensor_msgs::CompressedImageConstPtr& kinect2color_msg, const sensor_msgs::CompressedImageConstPtr&kinect2depth_msg, const sensor_msgs::ImuConstPtr& imu_msg, const nav_msgs::OdometryConstPtr& odom_msg) { ROS_INFO("Enter Publish"); bag_record.write("message_filter/velodyne_points",point_cloud2->header.stamp.now(),*point_cloud2); bag_record.write("message_filter/scan",laser_scan->header.stamp.now(),*laser_scan); bag_record.write("message_filter/camera/compressed",ominivision_msg->header.stamp.now(),*ominivision_msg); bag_record.write("message_filter/kinect2/qhd/image_color_rect/compressed",laser_scan->header.stamp.now(),*kinect2color_msg); bag_record.write("message_filter/kinect2/qhd/image_depth_rect/compressed",laser_scan->header.stamp.now(),*kinect2depth_msg); bag_record.write("message_filter/imu/data",imu_msg->header.stamp.now(),*imu_msg); bag_record.write("message_filter/odom",odom_msg->header.stamp.now(),*odom_msg); } int main(int argc, char** argv) { ros::init(argc, argv, "message_filter_node"); ros::Time::init(); ros::NodeHandle nh; ROS_INFO("start message filter"); time_t t=std::time(0); struct tm * now = std::localtime( & t ); string file_name; //the name of bag file is better to be determined by the system time file_name=int2string(now->tm_year + 1900)+ '-'+int2string(now->tm_mon + 1)+ '-'+int2string(now->tm_mday)+ '-'+int2string(now->tm_hour)+ '-'+int2string(now->tm_min)+ '-'+int2string(now->tm_sec)+ ".bag"; bag_record.open(file_name,rosbag::bagmode::Write); message_filters::Subscriber<sensor_msgs::PointCloud2> Velodyne_sub(nh, "/velodyne_points", 1);//订阅16线激光雷达Topic message_filters::Subscriber<sensor_msgs::LaserScan> Hokuyo_sub(nh,"/scan" , 1);//订阅平面激光雷达Topic message_filters::Subscriber<sensor_msgs::CompressedImage> ominivision_sub(nh,"/camera/image_raw/compressed" , 1);//订阅全向视觉Topic message_filters::Subscriber<sensor_msgs::CompressedImage> kinect2color_sub(nh,"/kinect2/qhd/image_color_rect/compressed" , 1);//订阅Kinect的Topic message_filters::Subscriber<sensor_msgs::CompressedImage> kinect2depth_sub(nh,"/kinect2/qhd/image_depth_rect/compressed" , 1);//订阅Kinect的Topic message_filters::Subscriber<sensor_msgs::Imu> imu_sub(nh,"/imu/data" , 1);//订阅imu的Topic message_filters::Subscriber<nav_msgs::Odometry> odom_sub(nh,"/odom" , 1);//订阅里程计的Topic typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::LaserScan, sensor_msgs::CompressedImage, sensor_msgs::CompressedImage, sensor_msgs::CompressedImage, sensor_msgs::Imu, nav_msgs::Odometry> MySyncPolicy; // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10) message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(20), Velodyne_sub, Hokuyo_sub, ominivision_sub, kinect2color_sub, kinect2depth_sub, imu_sub, odom_sub); sync.registerCallback(boost::bind(&callback, _1, _2, _3, _4, _5, _6, _7)); ros::spin(); bag_record.close(); return 0; }
这个框架直接拿出来就能用,同步过后的message会自动进入callback函数,之前把它封装成类结果一直跑不通,因为其中有些句柄当作了局部变量,这一点需要注意。