关于ROS在一个回调函数中处理两个订阅话题消息（多话题回调、多参数回调问题）

最近遇到需要在一个回调函数中处理多话题消息的问题，特此记录下在一个回调函数中处理多话题、多参数的方法。

举例：

#include <iostream>
#include <boost/bind.hpp>

#include <ros/ros.h>

#include <sensor_msgs/Image.h>
#include "detectmsg/SpatialDetection.h"
#include "detectmsg/SpatialDetectionArray.h"
#include "detectmsg/pose2D.h"
#include "detectmsg/multi_pedestrian_pose.h"
 
ros::Publisher obj_pub_;
ros::Subscriber  obj_sub_;
ros::Subscriber  image_sub_;

int Class_ID;

std::string pub_topic;
std::string sub_topic;

void Callback(const detectmsg::SpatialDetectionArray::ConstPtr& detect, const sensor_msgs::Image::ConstPtr& image)
{
  ROS_INFO("ok!");
}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "obj_publish_node");
    ros::NodeHandle nh;

    int ErrorParam=0;

    ErrorParam += !nh.getParam("objdetectnode/Class_ID_", Class_ID);

    ErrorParam += !nh.getParam("objdetectnode/pub_topic_", pub_topic);
    ErrorParam += !nh.getParam("objdetectnode/sub_topic_", sub_topic);


    if(ErrorParam>0){
      ROS_INFO("Error Param Load! Error Number:%d",ErrorParam);
      return 1;
    }

    ROS_INFO("SUCCESSFULLY STARTED");

    obj_pub_ = nh.advertise<detectmsg::multi_pedestrian_pose>(pub_topic, 1);
    obj_sub_ = nh.subscribe<detectmsg::SpatialDetectionArray>(sub_topic, 1, boost::bind(&Callback, _1 , sensor_msgs::Image::ConstPtr()));
    image_sub_ = nh.subscribe<sensor_msgs::Image>("/yolov4_publisher/color/image", 1, boost::bind(&Callback, detectmsg::SpatialDetectionArray::ConstPtr(), _1));
   
    ros::spin();
    return 0;
}

这是一段没写完的代码，主要用来测试一下多参数回调问题，最关键的部分在订阅话题的两端代码上，其他的代码不用看，需要注意到地方会在下文提到。

1、要实现多话题回调需要包含头文件，对应代码：#include <boost/bind.hpp>

2、要实现两个话题传向一个回调函数，首先先分别写出两个订阅话题的代码，最常用的订阅话题的代码段格式为：

obj_sub_ = nh.subscribe<detectmsg::SpatialDetectionArray>(sub_topic, 1, &Callback);

要实现多话题回调需要使用到函数boost::bind()，这里可以参考上文的代码（以obj_sub_为例），将原来回调函数的位置替换为该函数，函数内有三个参数，第一个是这两个话题进行绑定后共同传入的回调函数名称，第二个参数是一个占位符，为下面的话题订阅占位，最后一位为传入当前语句订阅的话题，注意结尾的::ConstPtr()。

image_sub_的语句是类似的，唯一的区别是占位的时候需要注意占位符位置的变化以及消息格式已经变成了上一个话题的消息格式。这里我特意选择了两个不同的消息格式防止混淆，相信各位一对比就能很清楚的明白其中的规律。

最后就是注意下一下在回调函数的参数部分，两个参数的顺序不能颠倒，同时注意const。

---------------------------------------------------------------------------------------------------------------------------------

上面代码经过测试是正常运行的，但是我尝试进行三个话题的同步回调时遇到了问题，按照上述写法，在回调函数中每次只能读取到一个话题的消息，不知道是因为时间同步问题导致还是代码里逻辑有问题，总之运行是不正确的，运行有问题的代码如下：

    // obj_sub_ = nh.subscribe<yolov5_ros_msgs::BoundingBoxes>("/yolov5/BoundingBoxes", 1, 
    //     boost::bind(&Callback, _1 , sensor_msgs::Image::ConstPtr(),sensor_msgs::CameraInfo::ConstPtr()));
    // image_sub_ = nh.subscribe<sensor_msgs::Image>("/stereo_publisher/stereo/depth", 1, 
    //     boost::bind(&Callback, yolov5_ros_msgs::BoundingBoxes::ConstPtr(), _1,sensor_msgs::CameraInfo::ConstPtr()));
    // caminfo_sub_ = nh.subscribe<sensor_msgs::CameraInfo>("/stereo_publisher/stereo/camera_info", 1, 
    //     boost::bind(&Callback, yolov5_ros_msgs::BoundingBoxes::ConstPtr(), sensor_msgs::Image::ConstPtr(),_1));

这段代码语法上是没有问题的，是可以编译通过的，我因为该代码的不正确运行，报的段错误，找到错误花了些时间，有一定迷惑性。

找到错误后更换了订阅话题方法，新方法用到了message_filters这个库，没有安装的话用下面的代码安装即可：

 sudo apt-get install ros-melodic-message-filters
//中间部分换成自己的版本

使用时包含头文件并并在cmakelists文件中find package部分进行添加

//头文件
#include<message_filters/subscriber.h>
#include<message_filters/time_synchronizer.h>

----------------------------------------------------
//CMakelists.txt

find_package(catkin REQUIRED COMPONENTS
  ...
  message_filters
)

...

target_link_libraries(your_executable_name
  ...
  ${catkin_LIBRARIES}
)

cpp中使用方法，还是上面的例子：

    message_filters::Subscriber<yolov5_ros_msgs::BoundingBoxes> detect_sub(nh, "/yolov5/BoundingBoxes", 1);
    message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/stereo_publisher/stereo/depth", 1);
    message_filters::Subscriber<sensor_msgs::CameraInfo> caminfo_sub(nh, "/stereo_publisher/stereo/camera_info", 1);

    message_filters::TimeSynchronizer<yolov5_ros_msgs::BoundingBoxes, sensor_msgs::Image, sensor_msgs::CameraInfo> sync(detect_sub, image_sub, caminfo_sub, 10);
    sync.registerCallback(boost::bind(&Callback, _1, _2, _3));