ros-多订阅

项目要求：同时订阅自定义消息和相机的图片这两个数据

自定义消息的定义

格式：Lanes：存放所有车道线的容器，Lane存放一条车道线的信息


注释：
（1）msg文件中如果在定义变量的时候，给该变量赋值，该变量的定义相当于宏定义
（2）如果需要将发布该自定义消息，需要在msg文件中加入std_msgs/Header header。加入这个header，使得该消息除了基本的自定义消息外，还包括stamp时间戳，frame_id坐标系名称。

在package.xml中添加功能包依赖

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

在CMakeList.txt添加编译选项

find_package(... 
message_generation   # 用于自定义消息
message_filters   # 用于多订阅  
)

add_message_files(FILES Lane.msg Lanes.msg)
generate_messages(DEPENDENCIES std_msgs)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES learning_topic
  CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim message_runtime
#  DEPENDS system_lib
)

实现发布者

以下的代码是先通过订阅来获取相机的图片，然后再将车道线的检测结果以自定义消息的形式发布出去

#include <camera_pkg/Lane.h>
#include <camera_pkg/Lanes.h>
camera_pkg::Lane lane;
camera_pkg::Lanes Lanes;

static ros::Publisher pubObCmdVel;
//定义回调函数
void imageCallback(const sensor_msgs::ImageConstPtr& msg,LaneDetection& lane_det,ros::Publisher& pubObCmdVel)
{
	startTime = clock();
	try
	{
		std::vector<Lane> result_lane_show = lane_det.LaneLineDetection(cv_bridge::toCvShare(msg, "bgr8")->image);
		for(int i=0;i<result_lane_show.size();i++)
		{
	             lane.is_pressed = result_lane_show[i].is_pressed;
		     lane.A = result_lane_show[i].A;
		     lane.B = result_lane_show[i].B;
		     lane.max_y = result_lane_show[i].max_y;
		     lane.min_y = result_lane_show[i].min_y;
		     lane.id = result_lane_show[i].id;
		     Lanes.Lanes.push_back(lane);
		}
		Lanes.header.stamp = ros::Time::now();
		pubObCmdVel.publish(Lanes);
		Lanes.Lanes.clear();
	}
	catch(cv_bridge::Exception& e)
	{
		ROS_ERROR("Could not convert from '%s' to 'bgr8'.",msg->encoding.c_str());
	}
}

int main(int argc, char** argv)
{
	LaneDetection lane_det;
	//初始化节点，定义节点名字
	ros::init(argc, argv, "image_sub_node");
	ros::NodeHandle n;
	//创建一个订阅者，订阅名为camera的话题(topic),　注册回调函数
	image_transport::ImageTransport it(n);
	Lanes.header.stamp = ros::Time::now();
	pubObCmdVel = n.advertise<camera_pkg::Lanes>("lane_data", 1);
	image_transport::Subscriber sub = it.subscribe("/usb_cam/image_raw", 1,boost::bind(imageCallback, _1, lane_det,pubObCmdVel));
	//循环等待回调函数
	ros::spin();
	cv::destroyWindow("view")；
}

注释：
通过Lanes.header.stamp = ros::Time::now();，以实现对自定义消息的时间戳赋值，必须要加这代码。

实现订阅者

以下代码实现订阅自定义消息的话题和获取相机图片的话题，回调函数被触发后，可以在该回调函数同时处理自定义消息的数据和相机图片数据

#include <ros/ros.h>
#include <camera_pkg/Lane.h>
#include <camera_pkg/Lanes.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
void callback(const camera_pkg::Lanes::ConstPtr& msg,const sensor_msgs::ImageConstPtr& a)
{
    ROS_INFO("---------------------data synchronization -------------------------");
}
int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "show_subscriber");

    // 创建节点句柄
    ros::NodeHandle n;

    message_filters::Subscriber<camera_pkg::Lanes>Lanes_data(n,"lane_data", 1);
    message_filters::Subscriber<sensor_msgs::Image>picture_data(n,"/usb_cam/image_raw", 1);  
    typedef sync_policies::ApproximateTime<camera_pkg::Lanes, sensor_msgs::Image> MySyncPolicy;  
    Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), Lanes_data, picture_data);
    sync.registerCallback(boost::bind(&callback, _1, _2));
    // 循环等待回调函数
    ros::spin();
    return 0;
}

注释

1. 有一种同步的方式：Time Synchronizer
2. 另外基于Policy-Based的消息同步机制，也是通过消息头部数据的时间辍进行同步。
   （1.） ExactTime：当所有需同步的话题的时间辍严格相等时，才会触发回调函数
   （2.） ApproximateTime：与需要时间辍完全相同的ExactTime不同，该方法允许话题之间的时间辍存在一定的偏差。
   本文采用的是ApproximateTime
3. 对于Synchronizer sync(MySyncPolicy(10), Lanes_data, picture_data)，对于 MySyncPolicy(10)的代表触发回调函数的频率要小于等于每秒10次----（如果有错，请大家纠正一下）

以上代码只供参考，未提供完整代码

参考链接：
ROS之多个订阅数据同步