消息同步机制.md

消息时间同步与回调

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

步骤:

       1.   message_filter ::subscriber 分别订阅不同的输入topic
       2.  TimeSynchronizer<Image,CameraInfo>   定义时间同步器;
       3.  sync.registerCallback      同步回调
       4.  void callback(const ImageConstPtr&image,const CameraInfoConstPtr& cam_info)    
           带多消息的消息同步自定义回调函数

      void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)  //回调中包含多个消息  
      message_filters::Subscriber<Image> image_sub(nh, "image", 1);             // topic1 输入  
      message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);   // topic2 输入  
      TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);       // 同步  
      sync.registerCallback(boost::bind(&callback, _1, _2));                   // 回调  

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

1. 头文件

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

2. 定义消息同步机制

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

3. 定义类成员变量

      message_filters::Subscriber<nav_msgs::Odometry>* odom_sub_ ; // topic1 输入  
      message_filters::Subscriber<sensor_msgs::Image>* img_sub_;   // topic2 输入  
      message_filters::Synchronizer<slamSyncPolicy>* sync_;  //同步器

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

      odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(ar_handle, "/odom", 1);  
      img_sub_  = new message_filters::Subscriber<sensor_msgs::Image>(ar_handle, "/usb_cam/image_raw", 1);  

      sync_ = new  message_filters::Synchronizer<slamSyncPolicy>(slamSyncPolicy(10), *odom_sub_, *img_sub_);  
      sync_->registerCallback(boost::bind(&QrSlam::combineCallback,this, _1, _2));  

参考

5. 类成员函数回调处理

  void QrSlam::combineCallback(const nav_msgs::Odometry::ConstPtr& pOdom, const sensor_msgs::ImageConstPtr& pImg)  //回调中包含多个消息  
  {  
      //TODO  
      fStampAll<<pOdom->header.stamp<<"    "<<pImg->header.stamp<<endl;  
      getOdomData(pOdom);                   //  
      is_img_update_ = getImgData(pImg);    // 像素值  
      cout << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta  
           << " " << robot_odom_.v << " " << robot_odom_.w << std::endl;  
      fOdom << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta  
            << " " << robot_odom_.v << " " << robot_odom_.w << std::endl;  
      pixDataToMetricData();  
      static bool FINISH_INIT_ODOM_STATIC = false;  
      if(FINISH_INIT_ODOM_STATIC)  
      {  
          ekfslam(robot_odom_);  
      }  
      else if(is_img_update_)  
      {  
          if(addInitVectorFull())  
          {  
              computerCoordinate();  
              FINISH_INIT_ODOM_STATIC = true;  
          }  
      }  
  }