ROS之订阅多个话题并对其进行同步处理(多传感器融合)

1.引言

本小节针对在ROS节点中需要订阅两个及两个以上的话题时，需要保持对这两个话题数据的同步，且需要同时接收数据一起处理然后当做参数传入到另一个函数中。
研究背景：realsenseT265 和 realsense D435 都有IMU数据，但是这两个传感器都将imu的数据拆开进行发布了，区分了线加速度和角加速，而在有一些场合我们需要合并使用。

2.方法一:利用全局变量TimeSynchronizer

• 1 . message_filter ::subscriber 分别订阅不同的输入topic

• 2 . TimeSynchronizer 定义时间同步器;

• 3 . sync.registerCallback 同步回调

• 4 . void callback(const ImageConstPtr&image,const CameraInfoConstPtr& cam_info) 带多消息的消息同步自定义回调函数

• 5 .相关API:http://docs.ros.org/api/message_filters/html/c++/classmessage__filters_1_1TimeSynchronizer.html
  

	#include 
	#include 
	#include 
	#include 
	
	using namespace message_filters;
	
	void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg_accel, const sensor_msgs::ImuConstPtr &imu_msg_gyro)
	{
		    double t = imu_msg_accel->header.stamp.toSec();
		    double dx = imu_msg_accel->linear_acceleration.x;
		    double dy = imu_msg_accel->linear_acceleration.y;
		    double dz = imu_msg_accel->linear_acceleration.z;
		    double rx = imu_msg_gyro->angular_velocity.x;
		    double ry = imu_msg_gyro->angular_velocity.y;
		    double rz = imu_msg_gyro->angular_velocity.z;
				Vector3d gyr(rx, ry, rz);
			Vector3d acc(dx, dy, dz);
		   /**
		  	处理函数 ......
		   */
	}
	
	int main(int argc, char** argv)
	{
		// 需要用message_filter容器对两个话题的数据发布进行初始化，这里不能指定回调函数
	    message_filters::Subscriber sub_imu_accel(n,IMU_TOPIC_ACCEL,2000,ros::TransportHints().tcpNoDelay());
	    message_filters::Subscriber sub_imu_gyro(n,IMU_TOPIC_GYRO,2000,ros::TransportHints().tcpNoDelay());
  		
  		// 将两个话题的数据进行同步
		typedef sync_policies::ApproximateTime syncPolicy;
	  	Synchronizer sync(syncPolicy(10), sub_imu_accel, sub_imu_gyro);  
	  	// 指定一个回调函数，就可以实现两个话题数据的同步获取
	  	sync.registerCallback(boost::bind(&imu_callback, _1, _2));
	  	
	  	ros::spin();
	  	return 0;
 		}

3. CMakeLists.txt 和 packages.xml添加ROS包

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

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

---

补充:用 TimeSynchronizer 改写成类形式中间出现了一点问题.后就改写成message_filters::Synchronizer的形式

4.方法二: 利用类成员message_filters::Synchronizer

• 1 . 头文件

#include 
#include 
#include 

• 2 . 定义消息同步机制和成员变量

typedef message_filters::sync_policies::ApproximateTime slamSyncPolicy;
message_filters::Subscriber* odom_sub_ ;             // topic1 输入
message_filters::Subscriber* img_sub_;   // topic2 输入
message_filters::Synchronizer* sync_;

• 3 .类构造函数中开辟空间new

    odom_sub_ = new message_filters::Subscriber(ar_handle, "/odom", 1);
    img_sub_  = new message_filters::Subscriber(ar_handle, "/usb_cam/image_raw", 1);
   
    sync_ = new  message_filters::Synchronizer(slamSyncPolicy(10), *odom_sub_, *img_sub_);
    sync_->registerCallback(boost::bind(&QrSlam::combineCallback,this, _1, _2));

ekf-slam 代码样例

• 4.类成员函数回调处理

void QrSlam::combineCallback(const nav_msgs::Odometry::ConstPtr& pOdom, const sensor_msgs::ImageConstPtr& pImg)  //回调中包含多个消息
{
    //TODO
    fStampAll<header.stamp<<"    "<header.stamp<