ROS2 IMU 消息发布

ROS2 IMU 消息发布 仅做为笔记 学习用

本文源地址：

【ROS2】获取imu数据并可视化保姆级教程(C++)_imu可视化-CSDN博客

/*
* transform.hpp
*/
#include <string>
#include <ctype.h>
#include <float.h>
#include <math.h>
class transform_imu
{
private:
    char *stcTime;
    char *stcAcc;
    char *stcGyro;
    char *stcAngle;

public:
    struct Acc
    {
        double x;
        double y;
        double z;
    } acc{0, 0, 0};
    struct Gyro
    {
        double x;
        double y;
        double z;
    } gyro{0, 0, 0};
    struct Angle
    {
        double r;
        double p;
        double y;
    } angle{0, 0, 0};

    /*public:
     transform_imu(double,double,double);
     //构造函数,也可尝试用初始化列表方式*/

    void FetchData(auto &data, int usLength)
    {
        int index = 0;
        // char *pData_head = data;
        // printf("count%xn",*pData_head);
        printf("%xn", data[index]);
        printf("count%dn", usLength);
        while (usLength >= 11) // 一个完整数据帧11字节
        {
            printf("%xn", data[index + 1]);
            if (data[index] != 0x55) // 0x55是协议头
            {
                index++; // 指针(/索引)后移，继续找协议头
                usLength--;
                continue;
            }
            // for(int i = 0;i < 1000;i++){printf("oncen");}
            if (data[index + 1] == 0x50) // time
            {
                stcTime = &data[index + 2];
                // memcpy(&stcTime, &data[index + 2], 8);
            }
            else if (data[index + 1] == 0x51) // 加速度
            {
                stcAcc = &data[index + 2];
                // memcpy(&stcAcc, &pData_head[index + 2], 8);
                acc.x = ((short)((short)stcAcc[1] << 8 | stcAcc[0])) / 32768.00 * 16 * 9.8;
                acc.y = ((short)((short)stcAcc[3] << 8 | stcAcc[2])) / 32768.00 * 16 * 9.8;
                acc.z = ((short)((short)stcAcc[5] << 8 | stcAcc[4])) / 32768.00 * 16 * 9.8;
            }
            else if (data[index + 1] == 0x52)
            {
                stcGyro = &data[index + 2];
                // memcpy(&stcGyro, &pData_head[index + 2], 8);
                gyro.x = ((short)((short)stcGyro[1] << 8 | stcGyro[0])) / 32768.00 * 2000 / 180 * M_PI; // 弧度制
                gyro.y = ((short)((short)stcGyro[3] << 8 | stcGyro[2])) / 32768.00 * 2000 / 180 * M_PI;
                gyro.z = ((short)((short)stcGyro[5] << 8 | stcGyro[4])) / 32768.00 * 2000 / 180 * M_PI;
            }
            else if (data[index + 1] == 0x53)
            {
                stcAngle = &data[index + 2];
                // memcpy(&stcAngle, &pData_head[index + 2], 8);
                angle.r = ((short)((short)stcAngle[1] << 8 | stcGyro[0])) / 32768.00 * M_PI;
                angle.p = ((short)((short)stcAngle[3] << 8 | stcGyro[2])) / 32768.00 * M_PI;
                angle.y = ((short)((short)stcAngle[5] << 8 | stcGyro[4])) / 32768.00 * M_PI;
            }

            /*case 0x54: //磁力计
            memcpy(&stcMag, &pData_head[2], 8);
            mag.x = stcMag[0];
            mag.y = stcMag[1];
            mag.z = stcMag[2];*/
            // 这里我一开始用switch case的写法
            /*case 0x59: //四元数
            memcpy(&stcQuat, &pData_head[2], 8);
            quat.w = stcQuat[0] / 32768.00;
            quat.x = stcQuat[1] / 32768.00;
            quat.y = stcQuat[2] / 32768.00;
            quat.z = stcQuat[3] / 32768.00;*/

            // 这个型号imu传感器6轴，暂时不启用这些读取
            // default:printf("overn");

            printf("overn");
            usLength = usLength - 11;
            index += 11;
        }
    }
};


/*
* publisher_imu.cpp
*/
#include <chrono>
#include <math.h>
#include "serial/serial.h" //前面安装的ROS2内置串口包
#include <memory.h>
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/magnetic_field.hpp"
//#include "geometry_msgs/msg/detail/vector3__struct.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "nav_msgs/msg/odometry.hpp"
#include <string>
#include "rclcpp/rclcpp.hpp"

#include "transform.hpp"

serial::Serial ser;

using namespace std::chrono_literals;

class publisher_imu_node : public rclcpp::Node// 节点命名与类最好一致
{
public:
    std::string port;
    int baudrate;
    std::string imu_topic;
    std::string imu_offline_topic;
    transform_imu imu_fetch; // 初始值设为0
public:
    publisher_imu_node()
        : Node("publisher_imu_node")
    {
        int output_hz = 20; // 频率，看传感器说明书
        // timer_ms = millisecond(output_hz);
        std::chrono::milliseconds timer_ms{output_hz}; // 换算成ms
        port = "/dev/ttyUSB0";                         // 串口号，主机可查看
        baudrate = 9600;                               // 波特率，imu在波特率115200返回数据时间大概是1ms,9600下大概是10ms，看imu参数设置
        try
        {
            ser.setPort(port);
            ser.setBaudrate(baudrate);
            serial::Timeout to = serial::Timeout::simpleTimeout(500);
            ser.setTimeout(to);
            ser.open();
        }
        catch (serial::IOException &e)
        {
            RCLCPP_INFO(this->get_logger(), "Unable to open port ");
            return;
        }

        if (ser.isOpen())
        {
            RCLCPP_INFO(this->get_logger(), "Serial Port initialized");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Serial Port ???");
            return;
        }

        pub_imu = this->create_publisher<sensor_msgs::msg::Imu>("/imu_data", 20);
        //<sensor_msgs::msg::Imu>消息数据类型可自行查看对应文件，消息队列长度设20
        pub_imu_offline = this->create_publisher<sensor_msgs::msg::Imu>("/imu_offline_data", 20);
        // 这里创建了两个话题，一个是/imu_data，一个是/imu_offline_data

        // imu = transform_imu();//初始化对象
        // ser.flush();
        // int size;
        printf("Process working_1n");
        timer_ = this->create_wall_timer(500ms, std::bind(&publisher_imu_node::timer_callback, this));//std::chrono::milliseconds timer_ms{output_hz}; // 换算成ms
        printf("Process working_2n");   
    }

public:
    void timer_callback()
    {
        int count = ser.available(); // 读取到缓存区数据的字节数
        if (count != 0)
        {
            // ROS_INFO_ONCE("Data received from serial port.");
            int num;
            rclcpp::Time now = this->get_clock()->now(); // 获取时间戳
            std::vector<unsigned char> read_buf(count);//这里定义无符号，是应为read函数的形参是无符号
            //unsigned char read_buf[count]; // 开辟数据缓冲区，注意这里是无符号类型
            num = ser.read(&read_buf[0], count); // 读出缓存区缓存的数据，返回值为读到的数据字节数

            std::vector<char> read_buf_char(count);//vector转char类型
            for(int i = 0;i < count;i++)
            {
                read_buf_char[i] = (char)read_buf[i];
            }
            imu_fetch.FetchData(read_buf_char, num);
            sensor_msgs::msg::Imu imu_data;         //
            sensor_msgs::msg::Imu imu_offline_data; //
            //------------------imu data----------------
            imu_data.header.stamp = now;
            imu_data.header.frame_id = "imu_frame";

            imu_data.linear_acceleration.x = imu_fetch.acc.x;
            imu_data.linear_acceleration.y = imu_fetch.acc.y;
            imu_data.linear_acceleration.z = imu_fetch.acc.z;

            imu_data.angular_velocity.x = imu_fetch.angle.r * 180.0 / M_PI;
            imu_data.angular_velocity.y = imu_fetch.angle.p * 180.0 / M_PI;
            imu_data.angular_velocity.z = imu_fetch.angle.y * 180.0 / M_PI;//数据结构里没有储存欧拉角的变量名称，用angular_velocity.z凑合

            tf2::Quaternion curr_quater;
            curr_quater.setRPY(imu_fetch.angle.r, imu_fetch.angle.p, imu_fetch.angle.y);
            // 欧拉角换算四元数
            RCLCPP_INFO(this->get_logger(), "Publishing: '");
            //RCLCPP_INFO(this->get_logger(), "angle: x=%f, y=%f, z=%f",imu.angle.r, imu.angle.p, imu.angle.y);

            imu_data.orientation.x = curr_quater.x();
            imu_data.orientation.y = curr_quater.y();
            imu_data.orientation.z = curr_quater.z();
            imu_data.orientation.w = curr_quater.w();
            // RCLCPP_INFO(this->get_logger(), "Quaternion: x=%f, y=%f, z=%f, w=%f",
            // imu_data.orientation.x, imu_data.orientation.y, imu_data.orientation.z, imu_data.orientation.w);

            //---------------imu offline data--------------
            imu_offline_data.header.stamp = now;
            // imu_offline_data.header.frame_id = imu_frame;

            imu_offline_data.linear_acceleration.x = imu_fetch.acc.x;
            imu_offline_data.linear_acceleration.y = imu_fetch.acc.y;
            imu_offline_data.linear_acceleration.z = imu_fetch.acc.z;

            imu_offline_data.angular_velocity.x = imu_fetch.gyro.x;
            imu_offline_data.angular_velocity.y = imu_fetch.gyro.y;
            imu_offline_data.angular_velocity.z = imu_fetch.gyro.z;

            imu_offline_data.orientation.x = curr_quater.x();
            imu_offline_data.orientation.y = curr_quater.y();
            imu_offline_data.orientation.z = curr_quater.z();
            imu_offline_data.orientation.w = curr_quater.w();

            pub_imu->publish(imu_data);
            pub_imu_offline->publish(imu_offline_data); // 发布话题，往两个话题放数据
        }
    }
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu_offline;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<publisher_imu_node>()); // 单线程，调用所有可触发的回调函数，将进入循环，不会返回
    printf("Process exitn");
    rclcpp::shutdown();
    return 0;
}