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

1.引言

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

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

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

• 2 . TimeSynchronizer<Image,CameraInfo> 定义时间同步器;

• 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 <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>

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

---

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

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

• 1 . 头文件

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

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

typedef message_filters::sync_policies::ApproximateTime<nav_msgs::Odometry,sensor_msgs::Image> slamSyncPolicy;
message_filters::Subscriber<nav_msgs::Odometry>* odom_sub_ ;             // topic1 输入
message_filters::Subscriber<sensor_msgs::Image>* img_sub_;   // topic2 输入
message_filters::Synchronizer<slamSyncPolicy>* sync_;

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

    odom_sub_ = new message_filters::Subscriber<nav_msgs::Odometry>(ar_handle, "/odom", 1);
    img_sub_  = new message_filters::Subscriber<sensor_msgs::Image>(ar_handle, "/usb_cam/image_raw", 1);
   
    sync_ = new  message_filters::Synchronizer<slamSyncPolicy>(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<<pOdom->header.stamp<<"    "<<pImg->header.stamp<<endl;
    getOdomData(pOdom);                   //
    is_img_update_ = getImgData(pImg);    // 像素值
    cout << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta
         << " " << robot_odom_.v << " " << robot_odom_.w << std::endl;
    fOdom << "stamp x y theta v w " << robot_odom_.stamp<<" "<<robot_odom_.x << " "<< robot_odom_.y << " " << robot_odom_.theta
          << " " << robot_odom_.v << " " << robot_odom_.w << std::endl;
    pixDataToMetricData();
    static bool FINISH_INIT_ODOM_STATIC = false;
    if(FINISH_INIT_ODOM_STATIC)
    {
        ekfslam(robot_odom_);
    }
    else if(is_img_update_)
    {
        if(addInitVectorFull())
        {
            computerCoordinate();
            FINISH_INIT_ODOM_STATIC = true;
        }
    }
}