ros消息時間同步與回調

轉載自http://blog.csdn.net/zyh821351004/article/details/47758433

 

方式一: 全局變量形式  : 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

//wiki參考demo http://wiki.ros.org/message_filters
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>

using namespace sensor_msgs;
using namespace message_filters;

void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)  //回調中包含多個消息
{
  // Solve all of perception here...
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");

  ros::NodeHandle nh;

  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));                   // 回調

  ros::spin();

  return 0;
}
// 

參考連接:http://wiki.ros.org/message_filters

 

方式二: 類成員的形式  message_filters::Synchronizer

說明: 我用 TimeSynchronizer 改寫成類形式中間出現了一點問題.后就改寫成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;
        }
    }
}