ROS之多传感器融合算法实现

ROS之多传感器融合算法实现

1、motivation

IntelRealsenseD435i传感器对imu传感器的数据是分开发布的，一般的SLAM的算法都是作为一个话题发布，所以要对它进行修改，用实际的传感器来说只需要修改launch文件，然后一并发布出去即可。

对于网上的一些公开数据集合而言，例如OpenLIROS数据集，在运行SLAM算法时需要进行处理，主要是两种解决办法

• 2.1中的方法，在SLAM算法源码上修改，保证时间同步，然后进行处理
• 2.2中的方法，把两个imu话题抓换成一个话题并发布出去

2、method

2.1 订阅ROS的多个话题并对数据进行处理

/*
* @Description:
订阅 ROS 的话题对数据进行处理(多传感器融合)
* @version: v1.0
* @Author: sunshine
* @Github: https://subshine.github.io/
* @Email: 2182216077@ncepu.edu.cn
* @Date: 2020-03-26 15:34:41
* @LastEditTime: 2020-03-27 17:53:23
*/
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/String.h>
#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) {
/*
处理程序
//
sensor_msgs::Imu imu_data;
imu_data.header.stamp = imu_msg_accel->header.stamp;
i