ROS的多传感器时间同步机制Time Synchronizer

1、存在的问题

多传感器数据融合的时候，由于各个传感器采集数据的频率的不同，例如odom 50Hz、Imu 100Hz、camera 25Hz，需要将传感器数据进行时间同步后才能进行融合。

2、融合的原理：

An example is the time synchronizer, which takes in messages of different types from multiple sources, and outputs them only if it has received a message on each of those sources with the same timestamp.
分别订阅不同的需要融合的传感器的主题，通过TimeSynchronizer 统一接收多个主题，只有在所有的topic都有相同的时间戳时，才会产生一个同步结果的回调函数，在回调函数里处理同步时间后的数据。

注意

只有多个主题都有数据的时候才可以触发回调函数。如果其中一个主题的发布节点崩溃了，则整个回调函数永远无法触发回调。
当多个主题频率一致的时候也无法保证回调函数的频率等于订阅主题的频率，一般会很低。实际测试订阅两个需要同步的主题，odom 50Hz、imu 50Hz，而回调函数的频率只有24Hz左右。

3、具体实现

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

参考链接

https://blog.csdn.net/xingdou520/article/details/83783768
https://blog.csdn.net/zyh821351004/article/details/47758433