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

ROS进阶 同时被 2 个专栏收录
13 篇文章 5 订阅
28 篇文章 27 订阅

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;
        }
    }
}
©️2021 CSDN 皮肤主题: 数字50 设计师:CSDN官方博客 返回首页
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值