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}; // 换算成msport = "/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>消息数据类型可自行查看对应文件，消息队列长度设20pub_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}; // 换算成msprintf("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;
}
