返回专辑
·Johan·3 分钟阅读

message_filters:对齐的是时间,不是运气

先修 stamp 与帧率;Exact/Approximate 策略、slop 与丢弃率要可监控。

message_filters:对齐的是时间,不是运气

1. 融合「有时有、有时无」——先查 stamp

相机 30 Hz、激光 10 Hz 做融合,回调随机触发,RViz 里叠加层闪烁。调大 ApproximateTime 的 slop 到 0.5 s 后「稳定了」,但窄道通过时规划偶发撞框——配对成功的是时间差巨大的错帧。同步器救不了坏时间戳,只能把错误配对包装成成功。

2. 驱动侧:stamp 契约

每条传感链路应保证:stamp 单调、来源一致(全 sim time 或全 system time)、语义是「观测时刻」而非「回调时刻」。用 now() 填图像 header 是常见根因。

cpp
// 错误:回调时刻冒充观测时刻
msg->header.stamp = this->now();

// 正确:使用驱动/硬件时间,或 sim time 下的采集时刻
msg->header.stamp = rclcpp::Time(hardware_timestamp_ns);

迁移顺序:修 stamp → 测丢弃率 → 再调 slop。相反顺序把系统性时间错误参数化藏起来。Bag 回放时若配对率断崖,先查 use_sim_time/clock,再查 slop——仿真与真机时钟域混用是 replay 场景的头号坑。

3. ExactTime vs ApproximateTime

ExactTime:要求参与话题 stamp 完全一致。仅适用于硬件硬同步(同一 PPS、同一触发线)或单消息已融合的多通道数据。ApproximateTime:在 slop 窗口内找最近邻配对,是多传感器常态。

cpp
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>

using SyncPolicy = message_filters::sync_policies::ApproximateTime<
  sensor_msgs::msg::Image, sensor_msgs::msg::LaserScan>;

auto sync = std::make_shared<message_filters::Synchronizer<SyncPolicy>>(
  SyncPolicy(10), image_sub, scan_sub);
sync->setMaxIntervalDuration(rclcpp::Duration::from_seconds(0.05));
sync->registerCallback(&FusionNode::callback, this);

queue_size 对应可容忍的缓冲深度;无限加大只延后 OOM,不提高配对质量。slop 应能解释:「两传感器最大预期时间差 + 抖动」,例如 30 Hz 相机与 10 Hz 激光可取 50–100 ms 量级,需用数据验证。

4. TF 与同步的次序

配对成功后用 now() 查 TF,等于放弃观测时刻的几何一致性。应用图像(或主传感器)stamplookupTransform

cpp
tf_buffer_->lookupTransform(
  target_frame, source_frame,
  tf2::TimePoint(std::chrono::nanoseconds(image_msg->header.stamp.nanosec)),
  tf2::durationFromSec(0.1));

否则同步再准,投影仍落在错误位姿上。

5. 丢弃率进诊断

同步器应暴露或统计:配对成功率、丢弃数、配对后平均时间差。持续丢弃率 >10% 说明 stamp/帧率/slop 有结构性问题,不是「还能出图就行」。

cpp
// 回调内累计,定期发布到 diagnostic 或 /metrics
if (!sync_success) { dropped_pairs_++; }

融合质量报告应带丢弃率,避免用更脏的数据训练更自信的模型。Slop 单位是时间不是帧数——换帧率后同一 slop 覆盖的帧数变了,迁平台必须重测,不能复制粘贴参数。

6. 与 QoS、仿真时钟

订阅端 QoS 不兼容导致一路 silently 无数据,同步器表现为永久不匹配——先 ros2 topic info -v 查兼容。仿真未统一 use_sim_time 时,两路 stamp 分属两域,slop 再大也无法稳定配对。

7. 验收

  • 录 60 s bag:统计配对成功率、平均 |t₁−t₂|
  • 改驱动 stamp 前后对比:丢弃率应显著下降
  • 窄道/运动场景:融合结果无系统性空间偏差
  • 参数(slop、queue)写入系统文档,录包回放配对率与在线一致

回放 bag 时用 --clock--rate 改变时间轴,配对统计须注明播放条件,避免把播放器 artifact 当成算法回归。

8. 案例:slop 掩盖标定事故

某项目把 slop 调到 200 ms 后融合「稳定」,但外参实际错了 5 cm,靠错帧配对平均掉了误差。修 stamp 并将 slop 收回 80 ms 后,标定问题暴露——这才是同步器该做的事:暴露不一致,而不是熨平不一致。

9. 默认策略

先时间戳,后同步器;slop 可解释且可监控;TF 用观测 stamp;丢弃率进诊断。同步参数是系统契约,不是个人 RViz 能出图的隐式值。

10. 多路以上与 CPU

三路及以上 ApproximateTime 时,配对组合爆炸,回调频率可能远低于最慢一路。考虑在驱动层做硬件同步,或降频统一到主传感器。Python 节点更要注意 GIL 与回调耗时——同步成功但处理赶不上,等价于丢帧。C++ 侧可评估 message_filters 与自管 deque 的取舍,但自管前先证明 stamp 已干净,否则只是手写同样的错配对。

相关

也可以看看

← 全部文章

johan's blog