ROS之多传感器融合算法实现
IntelRealsenseD435i传感器对imu传感器的数据是分开发布的，一般的SLAM的算法都是作为一个话题发布，所以要对它进行修改，用实际的传感器来说只需要修改launch文件，然后一并发布出去即可。
对于网上的一些公开数据集合而言，例如OpenLIROS数据集，在运行SLAM算法时需要进行处理，主要是两种解决办法
/*
* @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;
imu_data.header.frame_id = imu_msg_accel->header.frame_id;
imu_data.orientation.x = 0;
imu_data.orientation.y = 0;
imu_data.orientation.z = 0;
imu_data.orientation.w = 0;
线加速度
//
imu_data.linear_acceleration.x = imu_msg_accel->linear_acceleration.x;
imu_data.linear_acceleration.y = imu_msg_accel->linear_acceleration.y;
imu_data.linear_acceleration.z = imu_msg_accel->linear_acceleration.z;
⻆速度
//
imu_data.angular_velocity.x = imu_msg_gyro->angular_velocity.x;
imu_data.angular_velocity.y = imu_msg_gyro->angular_velocity.y;
imu_data.angular_velocity.z = imu_msg_gyro->angular_velocity.z;
std::cout << "imu_data.linear_acceleration"
<< " " << imu_data.angular_velocity.x << " "
<< imu_data.angular_velocity.y << " "
<<imu_data.angular_velocity.z << " " << std::endl;
*/
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "subscribe_two_and_translate_pub_one");
ros::NodeHandle n;
// SubscribeAndPublish sub_to_pub;
message_filters::Subscriber<sensor_msgs::Imu> sub_imu_accel(
n, "/d400/accel/sample", 2000,ros::TransportHints().tcpNoDelay());
message_filters::Subscriber<sensor_msgs::Imu> sub_imu_gyro(
n, "/d400/gyro/sample", 2000,ros::TransportHints().tcpNoDelay());
将两个话题的数据进行同步
//typedef sync_policies::ApproximateTime<sensor_msgs::Imu,sensor_msgs::Imu>syncPolocy;
Synchronizer<syncPolocy> sync(syncPolocy(10), sub_imu_accel,sub_imu_gyro);
指定一个回调函数,实现两个数据的同步读取
//
sync.registerCallback(boost::bind(&imu_Callback, _1, _2));
ros::spin();
return 0;
}
/*
* @Description: 把ROS bag包中的数据两个话题合并成一个话题
* @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 19:56:12
*/
#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;
class SubscribeAndPublish {
public:
ros::NodeHandle n;
ros::Publisher IMU_pub;
SubscribeAndPublish() {
IMU_pub = n.advertise<sensor_msgs::Imu>("/imu/data", 1);
message_filters::Subscriber<sensor_msgs::Imu> sub_imu_accel(
n, "/d400/accel/sample", 2000, ros::TransportHints().tcpNoDelay());
message_filters::Subscriber<sensor_msgs::Imu> sub_imu_gyro(
n, "/d400/gyro/sample", 2000, ros::TransportHints().tcpNoDelay());
//将两个话题的数据进行同步
typedef sync_policies::ApproximateTime<sensor_msgs::Imu,sensor_msgs::Imu>syncPolocy;
Synchronizer<syncPolocy> sync(syncPolocy(10), sub_imu_accel,sub_imu_gyro);
// //指定一个回调函数，实现两个数据的同步读取
sync.registerCallback(boost::bind(&SubscribeAndPublish::imu_Callback,this,_1, _2));
ros::spin();
}
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;
imu_data.header.frame_id = imu_msg_accel->header.frame_id;
imu_data.orientation.x = 0;
imu_data.orientation.y = 0;
imu_data.orientation.z = 0;
imu_data.orientation.w = 0;
//线加速度
imu_data.linear_acceleration.x = imu_msg_accel->linear_acceleration.x;
imu_data.linear_acceleration.y = imu_msg_accel->linear_acceleration.y;
imu_data.linear_acceleration.z = imu_msg_accel->linear_acceleration.z;
//角速度
imu_data.angular_velocity.x = imu_msg_gyro->angular_velocity.x;
imu_data.angular_velocity.y = imu_msg_gyro->angular_velocity.y;
imu_data.angular_velocity.z = imu_msg_gyro->angular_velocity.z;
IMU_pub.publish(imu_data);
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "subscribe_two_and_translate_pub_one");
SubscribeAndPublish sub_and_pub;
return 0;
}
实现效果：第一种办法应该不存在问题，第二种办法导致时间戳可能存在问题，具体看以下两张可视化图
原始数据
重新发布后的话题数据
参考链接： message_filters 对齐多种传感器数据的时间戳
C++:boost::bind的一些用法
ros::spin() 和 ros::spinOnce() 区别及详解