【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 header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance // Row major about x, y, z axes
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance // Row major about x, y, z axes
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance // Row major x, y z

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

---

2：发布Imu消息

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

#include 
#include 
#include 

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 &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("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.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数据的发布和订阅。