我的系统是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中的时间戳截图：
Screenshot from 2025-07-14 00-46-07.png
