ros消息时间同步与回调

转自：https://blog.csdn.net/zyh821351004/article/details/47758433

   最近做ekfslam,有两个数据输入/odom 与/ img信息,刚开始这两个都是通过rosbag包回放,分别有两个toptic订阅回调函数触发不同的处理.

这在后面处理的时候又需要对这两数据进行相应的时间同步,我之前采用的是odom建立vector向量表存储(频率比图像快),后面以img数据为准,

从img的时间戳搜索最近的odom. 然后再将这两数据传入ekfslam进行预测与更新. 而我这种处理方式又必须得考虑类成员与多线程(topic消息机制)

存在成员变量在过程中强行覆盖的问题. 

    今天与根哥讨论,建议我用消息同步的方式进行回调同步处理. 我这就参考wiki试着开干了.

    通过对多输入通道的输入topic的时间戳分析,进行同步,将同步的所有消息以一个回调的方式触发,

方式一: 全局变量形式  : TimeSynchronizer

步骤: 

     1.   message_filter ::subscriber 分别订阅不同的输入topic 

     2.  TimeSynchronizer<Image,CameraInfo>   定义时间同步器;

    3.  sync.registerCallback      同步回调

     4.  void callback(const ImageConstPtr&image,const CameraInfoConstPtr& cam_info)     带多消息的消息同步自定义回调函数

相应的API  message_filters::TimeSynchronizer

[html]  view plain  copy

1. //wiki参考demo http://wiki.ros.org/message_filters  
2. #include <message_filters/subscriber.h>  
3. #include <message_filters/time_synchronizer.h>  
4. #include <sensor_msgs/Image.h>  
5. #include <sensor_msgs/CameraInfo.h>  
6.   
7. using namespace sensor_msgs;  
8. using namespace message_filters;  
9.   
10. void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)  //回调中包含多个消息  
11. {  
12.   // Solve all of perception here...  
13. }  
14.   
15. int main(int argc, char** argv)  
16. {  
17.   ros::init(argc, argv, "vision_node");  
18.   
19.   ros::NodeHandle nh;  
20.   
21.   message_filters::Subscriber<Image> image_sub(nh, "image", 1);             // topic1 输入  
22.   message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);   // topic2 输入  
23.   TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);       // 同步  
24.   sync.registerCallback(boost::bind(&callback, _1, _2));                   // 回调  
25.   
26.   ros::spin();  
27.   
28.   return 0;  
29. }  
30. //   

参考连接:http://wiki.ros.org/message_filters

方式二: 类成员的形式  message_filters::Synchronizer

说明: 我用 TimeSynchronizer 改写成类形式中间出现了一点问题.后就改写成message_filters::Synchronizer的形式.

1. 头文件

[html]  view plain  copy

1. #include <message_filters/subscriber.h>  
2. #include <message_filters/synchronizer.h>  
3. #include <message_filters/sync_policies/approximate_time.h>  

2. 定义消息同步机制

[html]  view plain  copy

1. typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry,sensor_msgs::Image> slamSyncPolicy;  

3. 定义类成员变量

[html]  view plain  copy

1. message_filters::Subscriber<nav_msgs::Odometry>* odom_sub_ ;             // topic1 输入  
2. message_filters::Subscriber<sensor_msgs::Image>* img_sub_;   // topic2 输入  
3. message_filters::Synchronizer<slamSyncPolicy>* sync_;  

4.类构造函数中开辟空间new

补： https://github.com/kintzhao/cv_ekfslam/blob/c00f1d539b7e5dabf4cd062c0d871340fc9d6355/qrslam/qrslam.cpp#L67

[html]  view plain  copy

1. odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(ar_handle, "/odom", 1);  
2. img_sub_  = new message_filters::Subscriber<sensor_msgs::Image>(ar_handle, "/usb_cam/image_raw", 1);  
3.   
4. sync_ = new  message_filters::Synchronizer<slamSyncPolicy>(slamSyncPolicy(10), *odom_sub_, *img_sub_);  
5. sync_->registerCallback(boost::bind(&QrSlam::combineCallback,this, _1, _2));  

5. 类成员函数回调处理

[html]  view plain  copy

1. void QrSlam::combineCallback(const nav_msgs::Odometry::ConstPtr& pOdom, const sensor_msgs::ImageConstPtr& pImg)  //回调中包含多个消息  
2. {  
3.     //TODO  
4.     fStampAll<<pOdom->header.stamp<<"    "<<pImg->header.stamp<<endl;  
5.     getOdomData(pOdom);                   //  
6.     is_img_update_ = getImgData(pImg);    // 像素值  
7.     cout << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta  
8.          << " " << robot_odom_.v << " " << robot_odom_.w << std::endl;  
9.     fOdom << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta  
10.           << " " << robot_odom_.v << " " << robot_odom_.w << std::endl;  
11.     pixDataToMetricData();  
12.     static bool FINISH_INIT_ODOM_STATIC = false;  
13.     if(FINISH_INIT_ODOM_STATIC)  
14.     {  
15.         ekfslam(robot_odom_);  
16.     }  
17.     else if(is_img_update_)  
18.     {  
19.         if(addInitVectorFull())  
20.         {  
21.             computerCoordinate();  
22.             FINISH_INIT_ODOM_STATIC = true;  
23.         }  
24.     }  
25. }