ROS多消息同步与多消息回调

0.存在的问题

多传感器数据融合的时候，由于各个传感器采集数据的频率的不同，例如odom 50Hz、Imu 100Hz、camera 25Hz，需要将传感器数据进行时间同步后才能进行融合。这时就需要我们使用数据融合。当然只有多个主题都有数据的时候才可以触发回调函数。如果其中一个主题的发布节点崩溃了，则整个回调函数永远无法触发回调。当多个主题频率一致的时候也无法保证回调函数的频率等于订阅主题的频率，一般会很低。实际测试订阅两个需要同步的主题，odom 50Hz、imu 50Hz，而回调函数的频率只有24Hz左右。

1. 全局变量形式 : TimeSynchronizer

步骤:

1. ​​message_filter ::subscriber​​ 分别订阅不同的输入topic
2. ​​TimeSynchronizer<Image,CameraInfo>​​ 定义时间同步器;
3. ​​sync.registerCallback​​ 同步回调
4. ​​void callback(const ImageConstPtr&image,const CameraInfoConstPtr& cam_info)​​ 带多消息的消息同步自定义回调函数

​​相应的API message_filters​​::TimeSynchronizer

//wiki参考demo http://wiki.ros.org/message_filters
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
 
using namespace sensor_msgs;
using namespace message_filters;
 
void callback(const ImageConstPtr& image, const CameraInfoConstPtr& cam_info)  //回调中包含多个消息
{
  // Solve all of perception here...
}
 
int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");
 
  ros::NodeHandle nh;
 
  message_filters::Subscriber<Image> image_sub(nh, "image", 1);             // topic1 输入
  message_filters::Subscriber<CameraInfo> info_sub(nh, "camera_info", 1);   // topic2 输入
  TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);       // 同步
  sync.registerCallback(boost::bind(&callback, _1, _2));                   // 回调
 
  ros::spin();
 
  return 0;
}

参考连接:​​http://wiki.ros.org/message_filters​​

2.类成员的形式 message_filters::Synchronizer

另外，ros还有一种Policy-Based的消息同步机制，本质与上述Time Synchronizer类似。它分为两种，ExactTime Policy 和ApproximateTime Policy。

修改一下CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  sensor_msgs
  message_filters //ADD
  OpenCV
  cv_bridge 
  image_transport
)

find_package(PCL 1.7 REQUIRED)
find_package(OpenCV REQUIRED)

target_link_libraries (syn ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_LIBRARIES})

include_directories(include ${catkin_INCLUDE_DIRS})
include_directories(include ${OpenCV_INCLUDE_DIRS})
include_directories(include ${PCL_INCLUDE_DIRS})

add_executable(hw_1 src/hw_1.cpp)
target_link_libraries (hw_1 ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

使用时间戳相近对齐

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


#include <iostream>

using namespace std;
using namespace sensor_msgs;
using namespace message_filters;

ros::Publisher PointCloudInfo_pub;
ros::Publisher ImageInfo_pub;

PointCloud2 syn_pointcloud;
Image syn_iamge;

void Syncallback(const PointCloud2ConstPtr& ori_pointcloud,const ImageConstPtr& ori_image)
{
    cout << "\033[1;32m Syn! \033[0m" << endl;
    syn_pointcloud = *ori_pointcloud;
    syn_iamge = *ori_image;
    cout << "syn pointcloud' timestamp : " << syn_pointcloud.header.stamp << endl;
    cout << "syn image's timestamp : " << syn_iamge.header.stamp << endl;
    PointCloudInfo_pub.publish(syn_pointcloud);
    ImageInfo_pub.publish(syn_iamge);
}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "hw1");
    ros::NodeHandle node;

    cout << "\033[1;31m hw1! \033[0m" << endl;

    // 建立需要订阅的消息对应的订阅器
    message_filters::Subscriber<PointCloud2> PointCloudInfo_sub(node, "/rslidar_points", 1);
    message_filters::Subscriber<Image> ImageInfo_sub(node, "/zed/zed_node/left/image_rect_color", 1);
    
    typedef sync_policies::ApproximateTime<PointCloud2, Image> MySyncPolicy; 
    
    Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), PointCloudInfo_sub, ImageInfo_sub); //queue size=10
    sync.registerCallback(boost::bind(&Syncallback, _1, _2));


    PointCloudInfo_pub = node.advertise<PointCloud2>("/djq_pc", 10);
    ImageInfo_pub = node.advertise<Image>("/djq_image", 10);

    ros::spin();
    return 0;
}

同步结果

3. 其他详解链接

​​https://www.coder.work/article/3156449​​