ROS时间同步----使用message_filters进行时间软同步

目录

一、Time Synchronizer（时间同步器）

二、Time Sequencer（时间序列器）

三、Cache（缓存）

四、Policy-Based Synchronizer（基于策略的同步器）

1.ExactTime Policy（准确时间政策）

2.ApproximateTime Policy（ 近似时间政策）

五、Chain

检索过滤器

六、测试代码

参考

---

        message_filters官网：message_filters - ROS Wiki

一、Time Synchronizer（时间同步器）

        该类型同步器用于通过消息头 header 中的时间戳同步多个通道的消息，并将同步后的消息以单个回调的形式输出。C++ 实现最多可以同步 9 个通道的消息。

• 和 ExactTime Policy 相同
• 要求消息具有完全相同的时间戳。
• 使用场景：当需要严格时间对齐的场景，例如同时拍摄的图像和相机信息。

        当 topic_1 和 topic_2 的频率均为 1hz 时，同步结果如下。由于定时器回调函数中发布在 topic_1 和 topic_2 中的消息的时间戳存在微小的时间偏差，导致没有完全相同的时间戳，所以不会进入同步器的回调函数中。

        当 timer1_sub 和 timer2_sub 都订阅 topic_1 时，消息的时间戳完全相同，同步结果如下： 

二、Time Sequencer（时间序列器）

        TimeSequencer 是一种 ROS 消息过滤器，用于确保消息按照时间戳的顺序触发回调。它的核心机制是通过引入延迟来缓冲和排序消息，从而在某些实时性要求不高的场景中提供严格的时间顺序保证。

• 延迟队列机制：

  • 构造时指定一个 ros::Duration 类型的延迟参数（delay）。
  • 消息在进入队列后，需等待其时间戳相对于当前时间滞后至少 delay，才会被触发回调。

• 时间排序：

  • 所有消息根据其时间戳在缓冲区内进行排序，确保回调以时间顺序调用。

• 回调触发规则：

  • 消息只有当其时间戳滞后当前时间 delay 时，回调函数才会被触发。
  • 一旦某条消息的回调被触发，则后续任何时间戳早于此消息的消息都会被直接丢弃。

• 丢弃规则：

  • 如果某条消息的时间戳早于已触发回调的消息的时间戳，则该消息立即丢弃。
  • 队列容量满时，新到的消息会按照 FIFO（先进先出）规则丢弃较旧的消息。

message_filters::Subscriber<std_msgs::String> sub(nh, "my_topic", 1);

// 使用 TimeSequencer，延迟 100ms，检查频率 10ms，队列容量 10
message_filters::TimeSequencer<std_msgs::String> seq(
    sub, 
    ros::Duration(0.1),   // 延迟时间
    ros::Duration(0.01),  // 更新频率
    10                    // 队列容量
);

// 注册回调函数
seq.registerCallback(myCallback);

三、Cache（缓存）

        Cache 是一种 ROS 消息过滤器，用于存储接收到的消息历史记录，方便用户对特定时间范围的消息进行查询。它可以用来缓存最近的消息，并支持通过时间戳提取所需的消息。

• 消息存储：

  • 使用环形缓冲区保存最新的 N 条消息，旧消息会被新的消息覆盖。

• 时间范围查询：

  • 用户可以通过调用 cache.getInterval(start, end) 方法，从缓存中提取指定时间范围的消息。

• 消息透传：

  • 接收到的消息会立即透传到回调函数中(Callback 函数在每条新消息到来时都会被调用)，同时也会添加到缓存中。

// C++
// 订阅 "my_topic"，队列大小为 1
message_filters::Subscriber<std_msgs::String> sub(nh, "my_topic", 1);

// 创建一个 Cache 缓存，存储最近 100 条消息
message_filters::Cache<std_msgs::String> cache(sub, 100);

// 注册回调函数
cache.registerCallback(myCallback);

# Python
# 创建一个 Subscriber
sub = message_filters.Subscriber('my_topic', String)

# 创建一个 Cache，存储最近 100 条消息
cache = message_filters.Cache(sub, 100)

# 注册回调函数
cache.registerCallback(my_callback)

• 时间戳处理：

  • 默认情况下，消息的时间戳从其 header 字段获取。
  • 如果消息类型没有 header 字段，可以使用 allow_headerless=True（Python 支持），让缓存以接收消息的当前 ROS 时间作为时间戳。

# Python
sub = message_filters.Subscriber('my_int_topic', std_msgs.msg.Int32)
cache = message_filters.Cache(sub, 100, allow_headerless=True)
# the cache assigns current ROS time as each message's timestamp

• 时间范围查询（Python 示例） 

start_time = rospy.Time.now() - rospy.Duration(5)  # 5 秒前
end_time = rospy.Time.now()                        # 当前时间

# 查询缓存中 5 秒前到现在的消息
messages = cache.getInterval(start_time, end_time)
for msg in messages:
    rospy.loginfo(f"Retrieved message: {msg.data}")

四、Policy-Based Synchronizer（基于策略的同步器）

        该类型同步器用于通过消息头 header 中的时间戳同步多个通道的消息，并将同步后的消息以单个回调的形式输出。C++ 实现最多可以同步 9 个通道的消息（对应源码如下）。

  template<class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8>
  Synchronizer(const Policy& policy, F0& f0, F1& f1, F2& f2, F3& f3, F4& f4, F5& f5, F6& f6, F7& f7, F8& f8)
  : Policy(policy)
  {
    connectInput(f0, f1, f2, f3, f4, f5, f6, f7, f8);
    init();
  }

1.ExactTime Policy（准确时间政策）

• 要求消息具有完全相同的时间戳。
• 使用场景：当需要严格时间对齐的场景，例如同时拍摄的图像和相机信息。

        当 topic_1 和 topic_2 的频率均为 1hz 时，同步结果如下。由于定时器回调函数中发布在 topic_1 和 topic_2 中的消息的时间戳存在微小的时间偏差，导致没有完全相同的时间戳，所以不会进入同步器的回调函数中。

        当 timer1_sub 和 timer2_sub 都订阅 topic_1 时，消息的时间戳完全相同，同步结果如下：

2.ApproximateTime Policy（ 近似时间政策）

• 允许时间戳有一定的偏差。
• 使用场景：当消息可能存在小时间差的情况下，例如不同设备的同步问题。

ApproximateTime结构体的构造函数如下：

// 路径：/opt/ros/noetic/include/message_filters/sync_policies/approximate_time.h
  
  ApproximateTime(uint32_t queue_size)
  : parent_(0)
  , queue_size_(queue_size)
  , enable_reset_(false)
  , num_reset_deques_(0)
  , num_non_empty_deques_(0)
  , pivot_(NO_PIVOT)
  , max_interval_duration_(ros::DURATION_MAX)
  , age_penalty_(0.1)
  , has_dropped_messages_(9, false)
  , inter_message_lower_bounds_(9, ros::Duration(0))
  , warned_about_incorrect_bound_(9, false)
  , last_stamps_(9, ros::Time(0, 0))
  {
    ROS_ASSERT(queue_size_ > 0);  // The synchronizer will tend to drop many messages with a queue size of 1. At least 2 is recommended.
  }

        其中 max_interval_duration_ 的默认值为 ros::DURATION_MAX，ros::DURATION_MAX 是 ROS 中表示时间的最大值。也就是说，默认情况下，ApproximateTime 并不会限制消息之间的时间间隔，允许任意间隔进行同步，直到你手动调用 setMaxIntervalDuration 方法设置一个更小的值。

        默认情况下，ApproximateTime 并不会限制消息之间的时间间隔，允许任意间隔进行同步。当 topic_1 和 topic_2 的频率均为 1hz 时，发布 topic_1 后沉睡0.5s 后再发布 topic_2，代码如下：

    timer1 = node_.createTimer(ros::Duration(1), &TimeSync::Timer1CB, this);
    ros::Duration(0.5).sleep(); // 睡眠
    timer2 = node_.createTimer(ros::Duration(1), &TimeSync::Timer2CB, this);

        此时，消息时间戳如下所示： 

        同步结果如下：

 

        此时，若当设置 max_interval_duration_ 为 0.1 ，即通过如下代码对 max_interval_duration_ 进行设置：

sync_policy.setMaxIntervalDuration(ros::Duration(0.1));  // 设置最大时间容忍为 0.1 秒

       同步结果如下，即没有符合要求的同步结果。 

• 在设置 max_interval_duration_ 为 0.1 的情况下进行如下实验。

        当 topic_1 和 topic_2 的频率均为 1hz 时，同步结果如下：

        当 topic_1 的频率为 1hz，topic_2 的频率为 10hz 时，同步结果如下：

         如果某些消息类型不包含 header 消息头， C++ 版本的 ApproximateTimeSynchronizer 默认情况下不支持添加此类消息。然而 Python 版本的同步器可以通过设置 allow_headerless=True 添加此类消息（例如 Int32、Float64等），此时以接收消息的当前 ROS 时间作为其时间戳header.stamp：

import message_filters
from std_msgs.msg import Int32, Float32

def callback(mode, penalty):
   # The callback processing the pairs of numbers that arrived at approximately the same time
 
   mode_sub = message_filters.Subscriber('mode', Int32)
   penalty_sub = message_filters.Subscriber('penalty', Float32)
   
   sync = message_filters.ApproximateTimeSynchronizer([mode_sub, penalty_sub], 10, 0.1, allow_headerless=True)
   sync.registerCallback(callback)
   rospy.spin()

       代码问题：

typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::PointStamped, geometry_msgs::PointStamped> MySyncPolicy;
MySyncPolicy sync_policy(10);                          
message_filters::Synchronizer<MySyncPolicy> sync(sync_policy, timer1_sub, timer2_sub);


以上代码报错： error: no matching function for call to ‘message_filters::sync_policies::ApproximateTime<>::ApproximateTime()’ 

        原因分析： 

1. sync_policy 是局部变量，作为 Synchronizer 构造函数的参数进行传递。
2. Synchronizer 的构造函数声明如下：

     template<class F0, class F1>
     Synchronizer(const Policy& policy, F0& f0, F1& f1)
     : Policy(policy)
     {
       connectInput(f0, f1);
       init();
     }

   它接收的是 Policy 的常引用（const Policy&）。
3. 然而，由于 sync_policy 是显式声明的局部变量，当传递到 Synchronizer 构造函数时，C++ 编译器可能会认为需要调用默认构造函数进行初始化，这导致了以下错误：

• ApproximateTime 没有默认构造函数（因为它需要一个 queue_size 参数）。
• 因此编译器报错：no matching function for call to 'ApproximateTime'。

五、Chain

        Chain 是 ROS 中的一个动态消息过滤器容器，用于串联多个单输入/单输出（简单）的过滤器。它提供了一种灵活的机制，可以在运行时动态添加过滤器，并按添加顺序自动连接这些过滤器。

• 动态添加过滤器：

  • 可以根据需要在运行时添加过滤器，而无需在编译时固定。

• 自动连接：

  • 过滤器会按照添加顺序自动连接，每个过滤器的输出作为下一个过滤器的输入。

• 检索过滤器：

  • 支持通过索引检索已添加的过滤器。

• 集中管理：

  • 通过一个 Chain 实例管理多个过滤器，简化了复杂过滤器链的配置。

以下示例展示了如何创建一个 Chain，并动态添加两个过滤器：

1. 一个消息订阅器 (Subscriber)。
2. 一个时间排序器 (TimeSequencer)。

#include <message_filters/subscriber.h>
#include <message_filters/time_sequencer.h>
#include <message_filters/chain.h>
#include <std_msgs/String.h>

void myCallback(const std_msgs::String::ConstPtr& msg)
{
    ROS_INFO("Filtered message: %s", msg->data.c_str());
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "chain_example");
    ros::NodeHandle nh;

    // 创建 Chain 容器
    message_filters::Chain<std_msgs::String> chain;

    // 添加订阅器过滤器
    chain.addFilter(boost::make_shared<message_filters::Subscriber<std_msgs::String>>(nh, "my_topic", 1));

    // 添加时间排序器过滤器
    chain.addFilter(boost::make_shared<message_filters::TimeSequencer<std_msgs::String>>(
        ros::Duration(0.1), ros::Duration(0.01), 10));

    // 注册回调函数
    chain.registerCallback(myCallback);

    ros::spin();

    return 0;
}

• 检索过滤器

    Chain 支持通过索引检索已添加的过滤器：

message_filters::Chain<std_msgs::String> chain;
size_t sub_index = chain.addFilter(boost::make_shared<message_filters::Subscriber<std_msgs::String>>(nh, "my_topic", 1));

// 检索已添加的过滤器
boost::shared_ptr<message_filters::Subscriber<std_msgs::String>> sub = chain.getFilter<message_filters::Subscriber<std_msgs::String>>(sub_index);

         getFilter<T>(index) 返回一个指向指定类型过滤器的 boost::shared_ptr    

六、测试代码

#include <ros/ros.h>
#include <iostream>
#include <geometry_msgs/PointStamped.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>


// #define TIME_SYNC
// #define EXACT_SYNC
#define APPRO_SYNC
 
 
class TimeSync {
  public:
    TimeSync(ros::NodeHandle &node) : node_(node) {};
 
  public:
    ros::Timer timer1, timer2;
    ros::Publisher timer1_pub, timer2_pub;
    
  public:
    bool Initialization();
    void Timer1CB(const ros::TimerEvent&);
    void Timer2CB(const ros::TimerEvent&);
    void TimeSynCB(const geometry_msgs::PointStampedConstPtr &point1, const geometry_msgs::PointStampedConstPtr &point2);
 
  private:  
    ros::NodeHandle node_;
};
 

bool TimeSync::Initialization() {
    // 2个定时器，以不同频率发布数据。定时器会以固定时间间隔触发一个回调函数
    // ros::Duration(1)： 表示定时器的触发间隔为 1秒
    // &TimeSynch::TimerCB： 指定定时器触发时调用的回调函数，分别是 Timer1CB 和 Timer2CB
    timer1 = node_.createTimer(ros::Duration(1), &TimeSync::Timer1CB, this);
    timer2 = node_.createTimer(ros::Duration(1), &TimeSync::Timer2CB, this);
  
    timer1_pub = node_.advertise<geometry_msgs::PointStamped>("topic_1", 10);
    timer2_pub = node_.advertise<geometry_msgs::PointStamped>("topic_2", 10);
   
    message_filters::Subscriber<geometry_msgs::PointStamped> timer1_sub(node_, "topic_1", 10);
    message_filters::Subscriber<geometry_msgs::PointStamped> timer2_sub(node_, "topic_2", 10);
 
// 测试 3 种类型时间同步器的效果
//   1、TimeSynchronizer 
#ifdef TIME_SYNC
    message_filters::TimeSynchronizer<geometry_msgs::PointStamped, geometry_msgs::PointStamped> sync(timer1_sub, timer2_sub, 10);
    // sync.registerCallback(boost::bind(&TimeSync::TimeSynCB, this, _1, _2));  // 也可行，但性能稍低（由于 boost::bind 的开销）
    sync.registerCallback(&TimeSync::TimeSynCB, this);
#endif

//   2、ExactTime Policy 
#ifdef EXACT_SYNC
    typedef message_filters::sync_policies::ExactTime<geometry_msgs::PointStamped, geometry_msgs::PointStamped> MySyncPolicy;
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), timer1_sub, timer2_sub);
    sync.registerCallback(&TimeSync::TimeSynCB, this);
#endif

//   3、ApproximateTime Policy
#ifdef APPRO_SYNC
    typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::PointStamped, geometry_msgs::PointStamped> MySyncPolicy;
    // 定义同步策略
    MySyncPolicy sync_policy(10);                            // 设置队列大小 queue_size 为10
    sync_policy.setMaxIntervalDuration(ros::Duration(0.1));  // 设置最大时间容忍为 0.1 秒

    // 创建同步器, 传递策略, 注册回调
    message_filters::Synchronizer<MySyncPolicy> sync(std::ref(sync_policy), timer1_sub, timer2_sub);
    sync.registerCallback(&TimeSync::TimeSynCB, this);
#endif

    // 注意！由于作用域原因，必须放在订阅函数之后（不能放在主函数中，否则无法订阅到话题）！
    ros::spin();
   
    std::cout << "Initialized." << std::endl;
    return true;
}
 

void TimeSync::Timer1CB(const ros::TimerEvent&) {
    geometry_msgs::PointStamped point;
    point.header.stamp = ros::Time::now();
    std::cout << "****timer1=" << point.header.stamp << std::endl;
    timer1_pub.publish(point);
}
 

void TimeSync::Timer2CB(const ros::TimerEvent&) {
    geometry_msgs::PointStamped point;
    point.header.stamp = ros::Time::now();
    std::cout << "++++timer2=" << point.header.stamp << std::endl;
    timer2_pub.publish(point);  
}
 

void TimeSync::TimeSynCB(const geometry_msgs::PointStampedConstPtr &point1, const geometry_msgs::PointStampedConstPtr &point2) {
    std::cout << "-----------------------------------" << std::endl;
    std::cout << "point1 time = " << point1->header.stamp << "  point2 time = " << point2->header.stamp << std::endl;
    std::cout << "dt: " << point1->header.stamp - point2->header.stamp << std::endl;
    std::cout << "-----------------------------------" << std::endl;
}
 
 
int main(int argc, char **argv) {
    ros::init(argc, argv, "time_sync");
    ros::NodeHandle node;
    TimeSync timeSync(node);
    timeSync.Initialization();
 
    // ros::spin(); // 不能放这儿！
 
    return true;
}
 

参考

        message_filters

        ros多传感器时间同步(message_filters)

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