liosam 中inertiallabs_ins的ins话题和要求的imu话题格式不符问题

本文链接：https://blog.csdn.net/baidu_37045192/article/details/128077208

针对liosam中使用Inertial Labs INS传感器时遇到的话题格式不兼容问题，参照https://github.com/VT-ASIM-LAB/inertiallabs_gnss_driver的il_ins.cpp，进行了适配调整，将原ins话题转换为符合sensor_msgs/imu标准的消息格式，并发布了新话题。同时，还涉及到了与sensor_msgs/NavSatFix话题的转换，以确保数据正确传输。

摘要由CSDN通过智能技术生成

参考 https://github.com/VT-ASIM-LAB/inertiallabs_gnss_driver
中的il_ins.cpp进行新话题sensor_msgs/imu的发布。

话题格式如下

$:~/catkin_ins_ws$ rosmsg show sensor_msgs/Imu
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Quaternion orientation
  float64 x
  float64 y
  float64 z
  float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
  float64 x
  float64 y
  float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
  float64 x
  float64 y
  float64 z
float64[9] linear_acceleration_covariance

另一个新话题sensor_msgs/NavSatFix的格式

$:~/catkin_ins_ws$ rosmsg show sensor_msgs/NavSatFix
uint8 COVARIANCE_TYPE_UNKNOWN=0
uint8 COVARIANCE_TYPE_APPROXIMATED=1
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN=2
uint8 COVARIANCE_TYPE_KNOWN=3
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
sensor_msgs/NavSatStatus status
  int8 STATUS_NO_FIX=-1
  int8 STATUS_FIX=0
  int8 STATUS_SBAS_FIX=1
  int8 STATUS_GBAS_FIX=2
  uint16 SERVICE_GPS=1
  uint16 SERVICE_GLONASS=2
  uint16 SERVICE_COMPASS=4
  uint16 SERVICE_GALILEO=8
  int8 status
  uint16 service
float64 latitude
float64 longitude
float64 altitude
float64[9] position_covariance
uint8 position_covariance_type

修改后的il_ins.cpp

#include<iostream>
#include<unistd.h>
#include<math.h>
#include<stdlib.h>
#include <ros/ros.h>

//Inertial Labs source header
#include "ILDriver.h"

//adding message type headers
#include <inertiallabs_msgs/sensor_data.h>
#include <inertiallabs_msgs/ins_data.h>
#include <inertiallabs_msgs/gps_data.h>
#include <inertiallabs_msgs/gnss_data.h>
#include <inertiallabs_msgs/marine_data.h>
//添加标准imu头文件
#include <sensor_msgs/Imu.h>
//添加标准gps头文件
#include <sensor_msgs/NavSatFix.h>
//Publishers

struct Context {
   
	ros::Publisher publishers[7];
	std::string imu_frame_id;
};

void publish_device(IL::INSDataStruct *data, void* contextPtr)
{
   
	Context * context = reinterpret_cast<Context*>(contextPtr);
	static int seq=0;
	seq++;

	inertiallabs_msgs::sensor_data msg_sensor_data;
	inertiallabs_msgs::ins_data msg_ins_data;
	inertiallabs_msgs::gps_data msg_gps_data;
	inertiallabs_msgs::gnss_data msg_gnss_data;
	inertiallabs_msgs::marine_data msg_marine_data;
	//添加sensor_msgs::NavSatFix和sensor_msgs::Imu msg_imu;
	sensor_msgs::Imu msg_imu;
	sensor_msgs::NavSatFix msg_GPSFix;

	ros::Time timestamp = ros::Time::now();
	//添加常量
	double g = 9.80655;
	double deg_to_rad = 0.0174533;

	if (context->publishers[0].getNumSubscribers() > 0)
	{
   
		msg_sensor_data.header.seq = seq;
		msg_sensor_data.header.stamp = timestamp;
		msg_sensor_data.header.frame_id = context->imu_frame_id;
		msg_sensor_data.Mag.x = data->Mag[0];
		msg_sensor_data.Mag.y = data->Mag[0];
		msg_sensor_data.Mag.z = data->Mag[0];
		msg_sen