【cartographer_ros】五：发布和订阅陀螺仪Imu信息

上一节介绍了里程计Odometry传感数据的订阅和发布。

本节会介绍陀螺仪Imu数据的发布和订阅。陀螺仪在cartographer中主要用于前端位置预估和后端优化。

目录

1：sensor_msgs/Imu消息类型

2：发布Imu消息

3：订阅Imu消息

---

1：sensor_msgs/Imu消息类型

在终端查看消息数据结构：

rosmsg show sensor_msgs/Imu

Odometry消息类型数据结构如下：

Header headergeometry_msgs/Quaternion orientationfloat64[9] orientation_covariance // Row major about x, y, z axesgeometry_msgs/Vector3 angular_velocityfloat64[9] angular_velocity_covariance // Row major about x, y, z axesgeometry_msgs/Vector3 linear_accelerationfloat64[9] linear_acceleration_covariance // Row major x, y z

其中linear_acceleration表示线加速度，angular_velocity表示角速度，orientation表示姿态，使用四元数表示。covariance表示对应协方差，体现各个数据的误差

---

2：发布Imu消息

陀螺仪用的是LPMS-IG1 RS232,这个陀螺仪同时能提供角速度 ，线加速度，和欧拉角。

#include <ros/ros.h>#include <tf/tf.h>#include <sensor_msgs/Imu.h>using namespace std;unsigned int step = 0;unsigned int data_i = 0;unsigned int data_len = 0;unsigned char handle_buf[2048];float acc[3];float gyo[3];float eular[3];void DataReceivedCallback(std::vector<unsigned char> &data){    unsigned char datasingle1;    for (size_t k = 0; k < data.size(); k++)    {        datasingle1 = data[k];        switch (step)        {        case 0:        {            if (datasingle1 == 0x3A)            {                step = 1;                data_i = 0;                memset(handle_buf, 0, 2048);            }            break;        }        case 1: // sensor id low        {            handle_buf[0] = datasingle1;            step = 2;            break;        }        case 2: // sensor id high        {            handle_buf[1] = datasingle1;            step = 3;            break;        }        case 3: //指令号 low        {            handle_buf[2] = datasingle1;            step = 4;            break;        }        case 4: //指令号 high        {            handle_buf[3] = datasingle1;            step = 5;            break;        }        case 5: //数据长度 low        {            handle_buf[4] = datasingle1;            data_len = datasingle1;            step = 6;            break;        }        case 6: //数据长度 high        {            handle_buf[5] = datasingle1;            data_len += (uint16_t)handle_buf[5] * 256;            if (data_len > 512)            {                step = 0;                cout << " data_len error : " << hex << datasingle1 << ",  " << data_len << std::endl;            }            else            {                if (data_len > 0)                {                    data_i = 0;                    step = 7;                }                else                {                    step = 0;                    cout << " data_len error : " << hex << datasingle1 << ",  " << data_len << std::endl;                }            }            break;        }        case 7:        {            handle_buf[data_i + 6] = datasingle1;            data_i++;            if (data_i >= data_len + 4) //完整一帧            {                //判断包尾                if ((handle_buf[data_len + 8] != 0x0D) && (handle_buf[data_len + 9] != 0x0A))                {                    step = 0;                    cout << " tail error : " << hex << handle_buf[data_len + 8] << ",  " << hex << handle_buf[data_len + 9] << std::endl;                    break;                }                uint16_t lrc = ((uint16_t)handle_buf[data_len + 7] * 256) + (uint16_t)handle_buf[data_len + 6];                //判断lrc                uint16_t sum_lrc = 0;                for (unsigned int i = 0; i < (6 + data_len); i++)                {                    sum_lrc += handle_buf[i];                }                if (lrc != sum_lrc)                {                    step = 0;                    cout << " crc error : " << lrc << ",  " << sum_lrc << std::endl;                    break;                }                //线加速度(含重力)                acc[0] = *((float *)&handle_buf[22]);                acc[1] = *((float *)&handle_buf[26]);                acc[2] = *((float *)&handle_buf[30]);                //角速度(陀螺仪I的输出)                gyo[0] = *((float *)&handle_buf[82]);                gyo[1] = *((float *)&handle_buf[86]);                gyo[2] = *((float *)&handle_buf[90]);                //欧拉角                eular[0] = *((float *)&handle_buf[146]);                eular[1] = *((float *)&handle_buf[150]);                eular[2] = *((float *)&handle_buf[154]);                step = 0;            }            break;        }        default:            break;        }    }}int main(int argc, char **argv){    ros::init(argc, argv, "Imu_publisher");    ros::NodeHandle n;    ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu", 50);    string device = "/dev/ttyUSB0";    int baud_rate = 921600;    int data_bits = 8;    int stop_bits = 0;    string parity = "n";    boost::shared_ptr<SerialPort> serialPort;    serialPort.reset(new SerialPort(device, baud_rate, data_bits, stop_bits, parity));    auto binding = bind(&DataReceivedCallback, this, std::placeholders::_1);    serialPort->setcallback(binding);    if (serialPort->Open())    {        serialPort->LoadConfig();        cout << "Init serial open success";    }    else        cout << "Init serial open false";    ros::Rate r(1.0);    while (n.ok())    {        ros::spinOnce();        sensor_msgs::Imu imu;        imu.header.stamp = ros::Time::now();        imu.header.frame_id = "base_link";        imu.linear_acceleration.x = acc[0] * (-9.8);        imu.linear_acceleration.y = acc[1] * (-9.8);        imu.linear_acceleration.z = acc[2] * (-9.8);        imu.angular_velocity.x = gyo[0] * 3.1415926 / 180.0;        imu.angular_velocity.y = gyo[1] * 3.1415926 / 180.0;        imu.angular_velocity.z = gyo[2] * 3.1415926 / 180.0;        imu.orientation = tf::createQuaternionMsgFromRollPitchYaw(eular[0], eular[1], eular[2]);        //发布Imu消息        imu_pub.publish(imu);        last_time = current_time;        r.sleep();    }}

SerialPort是自定义的串口通信类，附上代码:

SerialPort.h

SerialPort.cpp

---

3：订阅Imu消息

(1) 通过rosbag订阅

rostopic echo /imu

(2) 通过rviz查看
打开rviz

rosrun rviz rviz

Fixed Frame修改为base_link，添加Imu并将Topic设为/imu

(3) 编写程序打印

#include "ros/ros.h"#include "sensor_msgs/Imu.h"void imuCallback(const sensor_msgs::Imu::ConstPtr &msg){    ROS_INFO("imu: %f, %f, %f, %f, %f, %f, %f, %f, %f, %f", msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z,             msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z,             msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z);}int main(int argc, char **argv){    ros::init(argc, argv, "listener");    ros::NodeHandle node;    ros::Subscriber subimu = node.subscribe("imu", 1000, imuCallback);    ros::spin();    return 0;}

【完】

---

下一节会介绍路标Landmark数据的发布和订阅。