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