ROS软时间同步机制message_filters总结与测试

时间同步机制对于多机器人（主控）系统或多传感器信息融合非常重要，划分为硬同步和软同步。

• 硬同步从源头上同步时间，杜绝了传输延迟影响。
• 软同步时间由话题发布时刻决定，期间存在传感器到主控话题订阅节点的传输延迟在里面，这个延迟有时候会比较严重，此时如果通过软同步无法解决延迟问题，就需要采用硬同步方案。
• 注意区别采集时刻时间和到达时刻时间：在ROS中，我们假定发布时间即为采集时刻时间，订阅时间即为到达时刻时间，因此，时间戳近似机制（采用只基于时间戳信息的自适应算法来匹配多个话题之间的时间同步性）能够适用于时间传输网络或数据处理导致的延迟场景。

ROS提供了3种同步机制，其实本质上是2种：普通时间同步机制，时间戳完全相同机制，时间戳近似机制。前2种本质是一样的。在message_filters包中实现。

其实，在本篇学习了ROS时间同步机制的同时，也会学习到定时器和C++封装ROS知识。

目录

测试源码

运行结果

测试1：2个定时器频率，timer1=0.1s, timer2=0.5s

测试2：2个定时器频率，timer1=0.5s, timer2=0.5s

测试3：时间同步器订阅相同话题

---

测试源码

将ROS封装成C++形式了，与C有一些不同地方，尤其需要注意

#ifndef TIME_SYNCHRONIZER_H_
#define TIME_SYNCHRONIZER_H_

#include <ros/ros.h>
#include <ros/timer.h>
#include <geometry_msgs/PointStamped.h>
#include <message_filters/subscriber.h>

/* 以下2个头文件有何区别？ */
#include <message_filters/time_synchronizer.h>
// #include <message_filters/synchronizer.h>

#include <message_filters/sync_policies/approximate_time.h>
#include <iostream>

using std::cout;
using std::endl;

// #define TIME_SYNCH
#define EXACT_SYNCH
// #define APPRO_SYNCH

class TimeSynch {
  public:
    TimeSynch(ros::NodeHandle &node) : node_(node) {};

  public:
    ros::Publisher timer1_pub, timer2_pub;
    ros::Timer timer1;
    ros::Timer timer2;
    // message_filters::Subscriber timer1_sub, timer2_sub;
    // 先在类内定义
    message_filters::Subscriber<geometry_msgs::PointStamped> timer1_sub;
    
  public:
    bool Initialization();
    void Timer1Handler(const ros::TimerEvent&);
    void Timer2Handler(const ros::TimerEvent&);
    void TimeSynchHandler(const geometry_msgs::PointStampedConstPtr &p1, const geometry_msgs::PointStampedConstPtr &p2);

  private:  
    ros::NodeHandle node_;
    std::string topic_name1_, topic_name2;
};

bool TimeSynch::Initialization() {
  /* 2个定时器，以不同频率发布数据 */
  timer1 = node_.createTimer(ros::Duration(1), &TimeSynch::Timer1Handler, this);
  timer2 = node_.createTimer(ros::Duration(1), &TimeSynch::Timer2Handler, this);
  /* 2个订阅器，分别订阅上述2个定时器发布的话题 */
  timer1_pub = node_.advertise<geometry_msgs::PointStamped>("p1", 10);
  timer2_pub = node_.advertise<geometry_msgs::PointStamped>("p2", 10);

  // timer1_sub = node_.subscribe("p1", 10); // wrong!
  // 再在构造函数或初始化函数中进行初始化
  timer1_sub.subscribe(node_, "p1", 10);
  // 直接在构造函数或初始化函数中定义并初始化
  message_filters::Subscriber<geometry_msgs::PointStamped> timer2_sub(node_, "p2", 10);

/* 以下分别定义了3种类型时间同步器，测试其效果，使用同一个回调函数 */
/* 1、普通时间同步器 */
#ifdef TIME_SYNCH
  message_filters::TimeSynchronizer<geometry_msgs::PointStamped, geometry_msgs::PointStamped> time_synch(timer1_sub, timer2_sub, 10);
  // time_synch.registerCallback(boost::bind(&TimeSynch::TimeSynchHandler, _1, _2)); // 错误！
  time_synch.registerCallback(&TimeSynch::TimeSynchHandler, this);
#endif
/*2、时间同步器：要求时间戳完全相同 */
#ifdef EXACT_SYNCH
  using ExactSynch = message_filters::sync_policies::ExactTime<geometry_msgs::PointStamped, geometry_msgs::PointStamped>;
  message_filters::Synchronizer<ExactSynch> exact_synch(ExactSynch(10), timer1_sub, timer2_sub);
  exact_synch.registerCallback(&TimeSynch::TimeSynchHandler, this);
#endif
/*3、时间同步器：只要求时间戳近似即可 */
/* 默认近似范围多大？近似范围是否支持自定义？如何定义？
 */
#ifdef APPRO_SYNCH
  typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::PointStamped, geometry_msgs::PointStamped> ApproSynch;
  message_filters::Synchronizer<ApproSynch> appro_synch(ApproSynch(10), timer1_sub, timer2_sub);
  appro_synch.registerCallback(&TimeSynch::TimeSynchHandler, this);
#endif
  // 注意！由于作用域原因，必须放在订阅函数之后（不能放在主函数中，否则无法订阅到话题）！
  ros::spin();

  cout << "Initialized." << endl;
  return true;
}

void TimeSynch::Timer1Handler(const ros::TimerEvent&) {
  cout << "timer1=" << ros::Time::now() << endl;
  geometry_msgs::PointStamped p1;
  p1.header.stamp = ros::Time::now();
  timer1_pub.publish(p1);
}

void TimeSynch::Timer2Handler(const ros::TimerEvent&) {
  cout << "timer2=" << ros::Time::now() << endl;
  geometry_msgs::PointStamped p2;
  p2.header.stamp = ros::Time::now();
  timer2_pub.publish(p2);  
}

void TimeSynch::TimeSynchHandler(const geometry_msgs::PointStampedConstPtr &p1, const geometry_msgs::PointStampedConstPtr &p2) {
  cout << "p1 time = " << p1->header.stamp << "  p2 time = " << p2->header.stamp << endl;
}


int main(int argc, char **argv) {
    ros::init(argc, argv, "time_synch");
    ros::NodeHandle node;
    TimeSynch timeSynch(node);
    timeSynch.Initialization();

    // ros::spin(); // 不能放这儿！

    return true;
}

#endif

 

运行结果

测试1：2个定时器频率，timer1=0.1s, timer2=0.5s

从左到右：时间戳近似同步机制，时间戳完全相同机制，普通时间同步机制。

只有时间戳近似定时器能够订阅到2个时间戳大致相近的话题。其他2个机制只有在2个话题时间戳完全相同时才能订阅到（以下没有出现完全相同的时候，因此没有订阅到）

 

测试2：2个定时器频率，timer1=1s, timer2=1s

我们将2个定时器发布频率设定相同都为1秒，由于两个发布器发布时间不可能完全相同，会存在一点点时间偏差，导致时间戳完全相同机制和普通时间同步机制都无法订阅到2个话题

测试3：时间同步器订阅相同话题

之所以这样做，是为了保证“2”个话题时间戳完全相同，测试时间戳完全相同机制。

此时，时间戳完全相同机制和普通时间同步机制都可以订阅到话题进入回调函数了。话题时间戳完全相同。