多雷达时间同步（软同步）（速腾）

S1mplet

已于 2024-03-26 15:34:23 修改

阅读量1.6k

点赞数 7

文章标签： linux

于 2024-03-15 15:03:17 首次发布

本文链接：https://blog.csdn.net/weixin_44589524/article/details/136721028

版权

文章介绍了如何在项目实践中对8个速腾激光雷达（RS80和RSBP）进行软件时间同步，包括使用message_filters库的ApproximateTime策略，以及提供了一个C++代码示例来展示如何订阅和同步不同激光雷达的数据。

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

由于项目实践需要，有8个速腾激光雷达（4个rs80和4个rsbp）需要时间同步。

常见的时间同步方式有两种：硬件同步和软件同步，硬件同步有PPS同步和PTP同步，同时在速腾激光雷达技术手册中通常推荐使用GPS的PPS信号进行同步，可参考如下博客和照片

Hesai激光雷达使用PTP时间同步_ptp master-CSDN博客

由于设备已经安装好，故本文采用软件同步，具体步骤如下

1、录制激光雷达bag包，并查看各自扫描频率

rqt_bag <bag_name>

可以很明显的看到各激光雷达的发布频率相差还是很大的

2、写代码

sync.c如下

#include "sync.h"
subscriberANDpublisher::subscriberANDpublisher()
{
    // 订阅四个激光雷达话题
    lidar1_sub.subscribe(nh, "rs80_1/rslidar_points", 1);
    lidar2_sub.subscribe(nh, "rs80_2/rslidar_points2", 1);
    lidar3_sub.subscribe(nh, "rs80_3/rslidar_points3", 1);
    lidar4_sub.subscribe(nh, "rs80_4/rslidar_points4", 1);

    // 消息过滤器，使用 ApproximateTime 进行时间同步
    sync_.reset(new message_filters::Synchronizer<syncpolicy>(syncpolicy(10), lidar1_sub, lidar2_sub, lidar3_sub, lidar4_sub));
    sync_->registerCallback(boost::bind(&subscriberANDpublisher::callback, this, _1, _2, _3, _4));

    // 发布者
    lidar1_pub = nh.advertise<sensor_msgs::PointCloud2>("rs80_1/sync", 10);
    lidar2_pub = nh.advertise<sensor_msgs::PointCloud2>("rs80_2/sync", 10);
    lidar3_pub = nh.advertise<sensor_msgs::PointCloud2>("rs80_3/sync", 10);
    lidar4_pub = nh.advertise<sensor_msgs::PointCloud2>("rs80_4/sync", 10);
}

void subscriberANDpublisher::callback(const sensor_msgs::PointCloud2ConstPtr& pc1,
                                       const sensor_msgs::PointCloud2ConstPtr& pc2,
                                       const sensor_msgs::PointCloud2ConstPtr& pc3,
                                       const sensor_msgs::PointCloud2ConstPtr& pc4) {
    ROS_INFO("Received synchronized message!");
    lidar1_pub.publish(pc1);
    lidar2_pub.publish(pc2);
    lidar3_pub.publish(pc3);
    lidar4_pub.publish(pc4);
}

sync.h如下

#ifndef SUB_AND_PUB_H
#define SUB_AND_PUB_H

#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <sensor_msgs/PointCloud2.h>

class subscriberANDpublisher{
public:
    subscriberANDpublisher();
    void callback(const sensor_msgs::ImageConstPtr &image,
                  const sensor_msgs::PointCloud2ConstPtr &lidar1,
                  const sensor_msgs::PointCloud2ConstPtr &lidar2,
                  const sensor_msgs::PointCloud2ConstPtr &lidar3,
                  const sensor_msgs::PointCloud2ConstPtr &lidar4);

private:
    ros::NodeHandle nh;
    ros::Publisher camera_pub;
    ros::Publisher lidar1_pub;
    ros::Publisher lidar2_pub;
    ros::Publisher lidar3_pub;
    ros::Publisher lidar4_pub;
    message_filters::Subscriber<sensor_msgs::PointCloud2> lidar1_sub;
    message_filters::Subscriber<sensor_msgs::PointCloud2> lidar2_sub;
    message_filters::Subscriber<sensor_msgs::PointCloud2> lidar3_sub;
    message_filters::Subscriber<sensor_msgs::PointCloud2> lidar4_sub;
    message_filters::Subscriber<sensor_msgs::Image> camera_sub;

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
                                                            sensor_msgs::PointCloud2,
                                                            sensor_msgs::PointCloud2,
                                                            sensor_msgs::PointCloud2,
                                                            sensor_msgs::PointCloud2> syncpolicy;
    typedef message_filters::Synchronizer<syncpolicy> Sync;
    boost::shared_ptr<Sync> sync_;

};

#endif

main.c如下

#include <ros/ros.h>
#include "sync.h"

 
int main(int argc, char **argv) {
    ros::init(argc, argv, "node");
    subscriberANDpublisher sp;
    ROS_INFO("main done! ");
    ros::spin();
}