ROS編程(2): 訂閱多個話題並對其進行同步處理

1. 研究背景

本小節針對在ROS節點中需要訂閱兩個及兩個以上的話題時，需要保持對這兩個話題數據的同步，且需要同時接收數據一起處理然後當做參數傳入到另一個函數中。
研究背景：realsenseT265 和 realsense D435 都有IMU數據，但是這兩個傳感器都將imu的數據拆開進行發佈了，區分了線加速度和角加速，而在有一些場合我們需要合併使用。

2. 詳細代碼如下：

		#include <message_filters/subscriber.h>
		#include <message_filters/synchronizer.h>
		#include <message_filters/sync_policies/approximate_time.h>
		#include <boost/thread/thread.hpp>
		
		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<sensor_msgs::Imu> sub_imu_accel(n,IMU_TOPIC_ACCEL,2000,ros::TransportHints().tcpNoDelay());
		    message_filters::Subscriber<sensor_msgs::Imu> sub_imu_gyro(n,IMU_TOPIC_GYRO,2000,ros::TransportHints().tcpNoDelay());
	  		
	  		// 將兩個話題的數據進行同步
			typedef sync_policies::ApproximateTime<sensor_msgs::Imu, sensor_msgs::Imu> syncPolicy;
		  	Synchronizer<syncPolicy> 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下添加：
	 <build_depend>image_transport</build_depend>
	 <exec_depend>image_transport</exec_depend>