数据时间戳压缩、_ROS对齐多种传感器数据的时间戳message_filters

来源 · CSDN

作者 · suyunzzz

编辑 · 睿小妹

原文 · https://blog.csdn.net/suyunzzz/article/details/

105623020

同步lidar数据(30hz)和image数据(60hz)

参考: https://www.cnblogs.com/gdut-gordon/p/10293446.html

---

message_filters 对齐多种传感器数据的时间戳

联合标定三维雷达和IMU，第一步要先对齐两种传感信息的时间戳。

ros官网提供了message_filters用于对齐多种传感信息的时间戳。

http://wiki.ros.org/message_filters#Time_Synchronizer

注意，对齐传感信息时间戳有两种方式，一种是时间戳完全对齐 ExactTime Policy ，另一种是时间戳相近 ApproximateTime Policy ，前者更为严格。

本人选用时间戳相近的对齐方法，代码如下：

#include #include #include #include #include #include #include #include using namespace std;using namespace sensor_msgs;using namespace message_filters;  PointCloud2 syn_pointcloud;  Imu syn_imu;void callback(const PointCloud2ConstPtr& ori_pointcloud, const ImuConstPtr& ori_imu){  // Solve all of perception here...  syn_pointcloud = *ori_pointcloud;  syn_imu = *ori_imu;  cout << "syn velodyne points' timestamp : " << syn_pointcloud.header.stamp << endl;  cout << "syn Imu's timestamp : " << syn_imu.header.stamp << endl;  ROS_INFO("pointcloud stamp value is: %d", syn_pointcloud.header.stamp);  ROS_INFO("imu stamp value is: %d", syn_imu.header.stamp);}int main(int argc, char** argv){  ros::init(argc, argv, "msg_synchronizer");  ros::NodeHandle nh;  message_filters::Subscriber imu_sub(nh, "/imu/data", 1);  message_filters::Subscriber velodyne_sub(nh, "/velodyne_points", 1);  typedef sync_policies::ApproximateTime MySyncPolicy;      Synchronizer sync(MySyncPolicy(10), velodyne_sub, imu_sub); //queue size=10  sync.registerCallback(boost::bind(&callback, _1, _2));  ros::spin();  return 0;}

callback函数订阅的所有/topic必须有数据存在，否则调用失败。

---

把上述代码写成一个类。

#include #include #include #include #include #include #include using namespace std;class mySynchronizer{public:    ros::Publisher pubVelodyne;    ros::Publisher pubImu;    mySynchronizer();    ~mySynchronizer(){};    void callback(const sensor_msgs::PointCloud2::ConstPtr& ori_pointcloud, const sensor_msgs::Imu::ConstPtr& ori_imu);};void mySynchronizer::callback(const sensor_msgs::PointCloud2::ConstPtr& ori_pointcloud, const sensor_msgs::Imu::ConstPtr& ori_imu){    cout << "*******************" << endl;    sensor_msgs::PointCloud2 syn_pointcloud = *ori_pointcloud;    sensor_msgs::Imu syn_imu = *ori_imu;    ROS_INFO("pointcloud stamp value is: %f", syn_pointcloud.header.stamp.toSec());    ROS_INFO("imu stamp value is: %f", syn_imu.header.stamp.toSec());    pubVelodyne.publish(syn_pointcloud);    pubImu.publish(syn_imu);}mySynchronizer::mySynchronizer(){    ros::NodeHandle nh;    pubVelodyne = nh.advertise<:pointcloud2>("/Syn/imu/data", 1);    pubImu = nh.advertise<:imu>("/Syn/velodyne_points", 1);    message_filters::Subscriber<:imu> imu_sub(nh, "/imu/data", 1);    message_filters::Subscriber<:pointcloud2> velodyne_sub(nh, "/velodyne_points", 1);    typedef message_filters::sync_policies::ApproximateTime<:pointcloud2 sensor_msgs::imu> MySyncPolicy;    message_filters::Synchronizer sync(MySyncPolicy(10), velodyne_sub, imu_sub);    sync.registerCallback(boost::bind(&mySynchronizer::callback, this, _1, _2));    ros::spin();}int main(int argc, char** argv){    ros::init(argc, argv, "msg_synchronizer");    ROS_INFO("\033[1;32m---->\033[0m Sync msgs node Started.");    mySynchronizer wode;    // ros::spin();    return 0;}

其中，非常注意2点：

A

sync.registerCallback(boost::bind(&mySynchronizer::callback, this, _1, _2)); 由于callback函数是类内函数，所以要使用类内成员函数的写法； 加上this关键字； 参考： https://blog.csdn.net/Felaim/article/ details/ 78212738# comment Box

B

ros::spin(); 要放在订阅函数之后。如果放在主函数中，由于作用域的原因，会出现无法订阅/topic的错误。 参考： https://www.cnblogs.com/liu-fa/p/5925381.html

C

注意一些头文件要包含进去，还要知道对应话题的msg类型

#include #include #include #include #include #include #include 

---

CMakeLists.txt文件以及package.xml文件编写 参考：https://blog.csdn.net/qq_29797957/ article/details/96432898

  // CMakeLists.txt下添加：  find_package(catkin REQUIRED COMPONENTS    ....    image_transport    ....  )  // package.xml下添加：   image_transport   image_transport

· end · 推荐阅读：  最新 I 第四届中国人工智能与机器人开发者大会  课程 I 面向自动驾驶的点云处理技术(深度学习)  产品 I AMR(自主移动)机器人——Robuster MR-Z 产品 I STEP机器人控制零部件首发对外开放  产品 I Nvidia Jetson：开发者追梦“法宝” 干货 I 面向LiDAR点云表征的3D目标检测代码库  干货 I 柔性外骨骼Soft Exosuit 干货 I 卡尔曼滤波：从入门到精通2  干货 I 卡尔曼滤波：从入门到精通  战报 I 2020第三届中国人工智能与机器人开发者大会