使用Synchronizer类时间同步三个消息

最新推荐文章于 2023-11-10 09:50:03 发布

Panzer4501

最新推荐文章于 2023-11-10 09:50:03 发布

阅读量1k

点赞数

分类专栏：激光雷达相机融合文章标签：机器人 c++ ubuntu

本文链接：https://blog.csdn.net/weixin_42803820/article/details/127113959

版权

激光雷达相机融合专栏收录该内容

2 篇文章 0 订阅

订阅专栏

参考：https://answers.ros.org/question/60903/timesynchronizer-callback-problem/

需要包含以下三个头文件：

#include<message_filters/synchronizer.h>

#include<message_filters/subscriber.h>

#include<message_filters/sync_policies/approximate_time.h>

以一个激光雷达消息和两个图像话题消息为例：

class MultiFusion{
private:
    ros::NodeHandle private_node_;
    ros::NodeHandle nh;
    message_filters::Subscriber<sensor_msgs::Image> leftCamera_sub_;
    message_filters::Subscriber<sensor_msgs::Image> rightCamera_sub_;
    message_filters::Subscriber<livox_ros_driver2::CustomMsg> lidar_sub_;

public:
    MultiFusion();
    ~MultiFusion();

    void callback( const livox_ros_driver2::CustomMsgConstPtr& cloud_msg,
                                const sensor_msgs::ImageConstPtr& leftImage_msg,
                                const sensor_msgs::ImageConstPtr& rightImage_msg);
};

MultiFusion::MultiFusion() : private_node_("~") {

    leftCamera_sub_.subscribe(nh, "/zed/zed_node/left_raw/image_raw_color", 120);
    rightCamera_sub_.subscribe(nh, "/zed/zed_node/right_raw/image_raw_color", 120);
    lidar_sub_.subscribe(nh, "/livox/lidar", 40);
    typedef message_filters::sync_policies::ApproximateTime<livox_ros_driver2::CustomMsg, sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(40), lidar_sub_, leftCamera_sub_, rightCamera_sub_);
    sync.registerCallback(boost::bind(&MultiFusion::callback, this, _1, _2, _3));

    ros::spin();

}

MultiFusion::callback是多传感器融合的回调函数，下面测试了时间同步的结果：

void MultiFusion::callback( const livox_ros_driver2::CustomMsgConstPtr& cloud_msg,
                            const sensor_msgs::ImageConstPtr& leftImage_msg,
                            const sensor_msgs::ImageConstPtr& rightImage_msg){
    //激光雷达、视觉融合回调函数
    cout << fixed << setprecision(16);
    cout << "lidar_time" <<cloud_msg->header.stamp.toSec() << endl;
    cout << "leftImage_time" << leftImage_msg->header.stamp.toSec() << endl;
    cout << "rightImage_time" << rightImage_msg->header.stamp.toSec() << endl;
    cout << "process add\n";
}