ApproximateTime 同步不成功

[秋露凝雪]

我的系统是ubuntu20.04，用的ros是galactic,我在同步之前录制了一个bag包，包含6个相机和一个激光雷达的信息系，下面是bag包的相关信息：

c606@ubuntu:~/fyx/data_bag$ ros2 bag info rosbag2_2025_07_10-03_25_26

Files:             rosbag2_2025_07_10-03_25_26_0.db3
Bag size:          2.4 GiB
Storage id:        sqlite3
Duration:          17.252s
Start:             Jul 10 2025 03:25:28.197 (1752143128.197)
End:               Jul 10 2025 03:25:45.450 (1752143145.450)
Messages:          999
Topic information: Topic: /rslidar_points | Type: sensor_msgs/msg/PointCloud2 | Count: 173 | Serialization Format: cdr
                   Topic: /camera1/pylon_ros2_camera_node/image_raw | Type: sensor_msgs/msg/Image | Count: 135 | Serialization Format: cdr
                   Topic: /camera3/pylon_ros2_camera_node/image_raw | Type: sensor_msgs/msg/Image | Count: 137 | Serialization Format: cdr
                   Topic: /camera2/pylon_ros2_camera_node/image_raw | Type: sensor_msgs/msg/Image | Count: 123 | Serialization Format: cdr
                   Topic: /camera4/pylon_ros2_camera_node/image_raw | Type: sensor_msgs/msg/Image | Count: 144 | Serialization Format: cdr
                   Topic: /camera5/pylon_ros2_camera_node/image_raw | Type: sensor_msgs/msg/Image | Count: 146 | Serialization Format: cdr
                   Topic: /camera6/pylon_ros2_camera_node/image_raw | Type: sensor_msgs/msg/Image | Count: 141 | Serialization Format: cdr

接着我写了一个功能包，作用是接受相机和雷达的话题，将话题内容同步后再以话题的形式发送出去，在写好节点代码后，我先通过命令ros2 run sys_time sys_time_node开启节点，再通过ros2 bag record -o synced_bag /clock $(ros2 topic list | grep "synced")命令开启录制，最后通过ros2 bag play rosbag2_2025_07_10-03_25_26 --loop --clock --read-ahead-queue-size 10000命令播放原始bag包。

在播放bag包后我发现同步节点的回调函数没有被调用，结束录制命令后我通过rqt_bag synced_bag命令查看时间戳，发现没有时间戳，这证明同步未进行或者同步失败了。

下面是我同步节点的运行日志：
c606@ubuntu:~/fyx/e2e_ws$ ros2 run sys_time sys_time_node
[INFO] [1752477350.845923197] [multi_sensor_sync]: 多传感器同步节点已启动 (ApproximateTime策略)
[WARN] [1752477359.044116748] []: Messages of type 6 arrived out of order (will print only once)
[WARN] [1752477376.810937820] []: Messages of type 3 arrived out of order (will print only once)
[WARN] [1752477376.825926258] []: Messages of type 0 arrived out of order (will print only once)
[WARN] [1752477376.831166002] []: Messages of type 2 arrived out of order (will print only once)
[WARN] [1752477376.840478915] []: Messages of type 5 arrived out of order (will print only once)
[WARN] [1752477376.891147241] []: Messages of type 1 arrived out of order (will print only once)
[WARN] [1752477377.102514764] []: Messages of type 4 arrived out of order (will print only once)

下面是我的同步节点代码：

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

using ImageMsg = sensor_msgs::msg::Image;
using PointCloud2Msg = sensor_msgs::msg::PointCloud2;

class MultiSensorSync : public rclcpp::Node {
public:
  MultiSensorSync() : Node("multi_sensor_sync") {
    // 增强QoS配置 
    auto qos = rclcpp::SensorDataQoS()
               .keep_last(20)       // 增大缓存队列
               .reliable()          // 可靠传输模式
               .durability_volatile(); // 允许历史数据
    
    initPublishers(qos);
    initSubscribersAndSync(qos);
    
    RCLCPP_INFO(this->get_logger(), "🚀 多传感器同步节点已启动 (ApproximateTime策略)");
  }

private:
  void initPublishers(const rclcpp::QoS& qos) {
    for (int i = 0; i < 6; ++i) {
      cam_pubs_.push_back(
        this->create_publisher<ImageMsg>("/camera" + std::to_string(i+1) + "/synced/image_raw", qos)
      );
    }
    lidar_pub_ = this->create_publisher<PointCloud2Msg>("/rslidar_points/synced", qos);
  }

  void initSubscribersAndSync(const rclcpp::QoS& qos) {
    // 创建相机订阅器
    for (int i = 0; i < 6; ++i) {
      cam_subs_.push_back(
        std::make_shared<message_filters::Subscriber<ImageMsg>>(
          this, 
          "/camera" + std::to_string(i+1) + "/pylon_ros2_camera_node/image_raw",
          qos.get_rmw_qos_profile()
        )
      );
    }
    
    // 创建雷达订阅器
    lidar_sub_ = std::make_shared<message_filters::Subscriber<PointCloud2Msg>>(
      this, "/rslidar_points", qos.get_rmw_qos_profile()
    );
    
    // 配置ApproximateTime策略 
    using SyncPolicy = message_filters::sync_policies::ApproximateTime<
      ImageMsg, ImageMsg, ImageMsg, ImageMsg, ImageMsg, ImageMsg, PointCloud2Msg>;
    
    // 创建同步器（增大时间窗口）
    sync_ = std::make_shared<message_filters::Synchronizer<SyncPolicy>>(
      SyncPolicy(50),  // 队列大小增至50
      *cam_subs_[0], *cam_subs_[1], *cam_subs_[2], 
      *cam_subs_[3], *cam_subs_[4], *cam_subs_[5], 
      *lidar_sub_
    );
    
    // 注册回调并绑定生命周期
    sync_->registerCallback(
      std::bind(&MultiSensorSync::syncCallback, this, 
        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
        std::placeholders::_4, std::placeholders::_5, std::placeholders::_6,
        std::placeholders::_7
      )
    );
  }

  void syncCallback(
    const ImageMsg::ConstSharedPtr& cam1,
    const ImageMsg::ConstSharedPtr& cam2,
    const ImageMsg::ConstSharedPtr& cam3,
    const ImageMsg::ConstSharedPtr& cam4,
    const ImageMsg::ConstSharedPtr& cam5,
    const ImageMsg::ConstSharedPtr& cam6,
    const PointCloud2Msg::ConstSharedPtr& lidar)
  {
    // 使用第一个相机的时间戳作为基准
    //const auto& base_stamp = cam1->header.stamp;
    const auto& base_stamp = lidar->header.stamp;
    // 发布所有相机数据（统一时间戳）
    publishWithStamp(cam_pubs_[0], cam1, base_stamp);
    publishWithStamp(cam_pubs_[1], cam2, base_stamp);
    publishWithStamp(cam_pubs_[2], cam3, base_stamp);
    publishWithStamp(cam_pubs_[3], cam4, base_stamp);
    publishWithStamp(cam_pubs_[4], cam5, base_stamp);
    publishWithStamp(cam_pubs_[5], cam6, base_stamp);
    
    // 发布雷达数据（统一时间戳）
    auto new_lidar = std::make_shared<PointCloud2Msg>(*lidar);
    new_lidar->header.stamp = base_stamp;
    lidar_pub_->publish(*new_lidar);
    
    // 修复时间戳访问方式
    RCLCPP_INFO(this->get_logger(), "🔥 同步发布 @ %d.%09d", 
               base_stamp.sec, base_stamp.nanosec);
  }

  void publishWithStamp(rclcpp::Publisher<ImageMsg>::SharedPtr pub, 
                        const ImageMsg::ConstSharedPtr& msg, 
                        const builtin_interfaces::msg::Time& stamp) {
    auto new_msg = std::make_shared<ImageMsg>(*msg);
    new_msg->header.stamp = stamp;
    pub->publish(*new_msg);
  }

  // 成员变量
  std::vector<rclcpp::Publisher<ImageMsg>::SharedPtr> cam_pubs_;
  std::vector<std::shared_ptr<message_filters::Subscriber<ImageMsg>>> cam_subs_;
  rclcpp::Publisher<PointCloud2Msg>::SharedPtr lidar_pub_;
  std::shared_ptr<message_filters::Subscriber<PointCloud2Msg>> lidar_sub_;
  
  using SyncPolicy = message_filters::sync_policies::ApproximateTime<
    ImageMsg, ImageMsg, ImageMsg, ImageMsg, ImageMsg, ImageMsg, PointCloud2Msg>;
  std::shared_ptr<message_filters::Synchronizer<SyncPolicy>> sync_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  rclcpp::executors::MultiThreadedExecutor executor;
  auto node = std::make_shared<MultiSensorSync>();
  executor.add_node(node);
  executor.spin();
  rclcpp::shutdown();
  return 0;
}

最后还有一点，我录制的原始bag中相机和雷达的频率在10hz左右，下面是原始bag中的时间戳截图：