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

最新推荐文章于 2024-11-26 14:47:14 发布

东街听落雨

最新推荐文章于 2024-11-26 14:47:14 发布

阅读量6.1k

点赞数 10

CC 4.0 BY-SA版权

分类专栏： ROS 工程实践核心问题：时间同步

本文链接：https://blog.youkuaiyun.com/qq_18276949/article/details/113636353

ROS 同时被 2 个专栏收录

2 篇文章

订阅专栏

工程实践核心问题：时间同步

1 篇文章

订阅专栏

本文介绍ROS中的时间同步机制，包括硬同步和软同步，并详细解释了三种同步方式：普通时间同步机制、时间戳完全相同机制及时间戳近似机制。通过实例展示了不同同步方式的效果。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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

硬同步从源头上同步时间，杜绝了传输延迟影响。
软同步时间由话题发布时刻决定，期间存在传感器到主控话题订阅节点的传输延迟在里面，这个延迟有时候会比较严重，此时如果通过软同步无法解决延迟问题，就需要采用硬同步方案。
注意区别采集时刻时间和到达时刻时间：在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