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

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

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

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

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表示对应协方差，体现各个数据的误差

陀螺仪用的是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: //&#x6307;&#x4EE4;&#x53F7; low
        {
            handle_buf[2] = datasingle1;
            step = 4;
            break;
        }
        case 4: //&#x6307;&#x4EE4;&#x53F7; high
        {
            handle_buf[3] = datasingle1;
            step = 5;
            break;
        }
        case 5: //&#x6570;&#x636E;&#x957F;&#x5EA6; low
        {
            handle_buf[4] = datasingle1;
            data_len = datasingle1;
            step = 6;
            break;
        }
        case 6: //&#x6570;&#x636E;&#x957F;&#x5EA6; 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) //&#x5B8C;&#x6574;&#x4E00;&#x5E27;
            {
                //&#x5224;&#x65AD;&#x5305;&#x5C3E;
                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];

                //&#x5224;&#x65AD;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;
                }

                //&#x7EBF;&#x52A0;&#x901F;&#x5EA6;(&#x542B;&#x91CD;&#x529B;)
                acc[0] = *((float *)&handle_buf[22]);
                acc[1] = *((float *)&handle_buf[26]);
                acc[2] = *((float *)&handle_buf[30]);

                //&#x89D2;&#x901F;&#x5EA6;(&#x9640;&#x87BA;&#x4EEA;I&#x7684;&#x8F93;&#x51FA;)
                gyo[0] = *((float *)&handle_buf[82]);
                gyo[1] = *((float *)&handle_buf[86]);
                gyo[2] = *((float *)&handle_buf[90]);

                //&#x6B27;&#x62C9;&#x89D2;
                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]);

        //&#x53D1;&#x5E03;Imu&#x6D88;&#x606F;
        imu_pub.publish(imu);

        last_time = current_time;
        r.sleep();
    }
}
</serialport></sensor_msgs::imu></unsigned></sensor_msgs></tf></ros>

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

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

Original: https://www.cnblogs.com/CloudFlame/p/16443733.html
Author: CloudFlame
Title: 【cartographer_ros】五：发布和订阅陀螺仪Imu信息