ros2 c 实现JY_95T IMU解算三轴 加速度 角速度 欧拉角 磁力计 四元数

起因，机器人建图导航程序需要做里程计 imu数据融合，需要填充imu数据，但对imu填充的数据一直不是很了解，并且正在学习c++的类与对象，新近入手了一款JY_95T IMU，没有ros2的c++实现，所幸就拿它练练手，用c++实现读取imu数据。

JY_95T IMU 解算数据的python实现：

ros2小车串口安装GY_95T IMU陀螺仪加速度计初体验_JT_BOT的博客-CSDN博客

配置环境参考：

怎样愉快的连接使用usb转串口设备_JT_BOT的博客-CSDN博客

串口读取原理参考：

怎样愉快的使用串口发送16进制数据并读取串口内容_JT_BOT的博客-CSDN博客

JY_95T IMU九轴模块使用说明：

imu传感器数据解算原理：

 此款imu内置44个寄存器，每个寄存器存储一个字节的数据，对应于不同的功能，具体功能参考上面的使用说明，其中8-42号寄存器储存的是imu的3个轴的实时数据，我们的目的就是通过c++读取这35个寄存器数据并加以解算。从串口读取到的一个完整的数据帧包含40位数据，0-3位4个Byte是imu的配置信息，4-38位35个Byte是数据信息，39位是校验和低八位。下面的程序就围绕这个原理展开。

运行环境：ubuntu 20.04    ros2 foxy 

需要提前安装好ros2 ，不会安装的参考：

从零开始安装机器人foxy系统_foxy安装_JT_BOT的博客-CSDN博客

创建ros2工作空间：

mkdir -p ros2_ws/src   //创建ros2工作空间
 cd ros2_ws/src
 ros2 pkg create imu_get_cpp --build-type ament_cmake --dependencies rclcpp //创建功能包
 cd imu_get_cpp/src
 touch publisher_imu.cpp  //生成文件
 touch transform.hpp      //生成文件
 cd ~/ros2_ws             //返回工作空间
 colcon build             //编译
 

文件目录结构：

 publisher_imu.cpp

//publisher_imu.cpp
 #include 
 #include 
 #include "serial/serial.h" //导入串口库，ros2不带串口库，需要单独安装
 #include 
 #include "std_msgs/msg/string.hpp"
 #include "sensor_msgs/msg/magnetic_field.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 
 #include "rclcpp/rclcpp.hpp"#include "transform.hpp"serial::Serial ser;using namespace std::chrono_literals;class publisher_imu_node : public rclcpp::Node// 创建imu发布类继承自rclcpp::Node
 {
 private:std::string port;   //端口名int baudrate;       //波特率查看硬件说明transform_imu imu_fetch; //创建imu获取对象rclcpp::TimerBase::SharedPtr timer_;   //创建定时器rclcpp::Publisher::SharedPtr pub_imu; // 创建发布者
 public:publisher_imu_node(): Node("publisher_imu_node")  //构造函数{port = "/dev/ttyUSB0";  // 根据自己的主机修改串口号baudrate = 115200;      // 波特率，imu在波特率115200返回数据时间大概是1ms,9600下大概是10mstry{ser.setPort(port);           //设置端口ser.setBaudrate(baudrate);   //设置波特率serial::Timeout to = serial::Timeout::simpleTimeout(1); //设置超时ser.setTimeout(to);ser.open();  //打开串口}catch (serial::IOException &e){RCLCPP_INFO(this->get_logger(), "不能打开端口 ");return;}if (ser.isOpen()){RCLCPP_INFO(this->get_logger(), "串口初始化完成！");unsigned char cmd_buffer[5] ={0xA4,0x03,0x08,0x23,0xD2}; //JY-95T启用35个寄存器 需要发送的串口包命令ser.write(cmd_buffer,5);  //JY-95T可以根据命令选择工作方式printf("发出命令: %x %x %x %x %xn",cmd_buffer[0],cmd_buffer[1],cmd_buffer[2],cmd_buffer[3],cmd_buffer[4]);}else{RCLCPP_INFO(this->get_logger(), "Serial Port ???");return;}pub_imu = this->create_publisher("/imu_data", 20);//创建了话题 /imu_data// 创建定时器，100ms为周期，定时发布，这个时间设置超过200毫秒发布的数据卡顿timer_ = this->create_wall_timer(100ms, std::bind(&publisher_imu_node::timer_callback, this));printf("发布线程工作中n");}public:void timer_callback()   //定时器的回调函数，每20毫秒回调一次{int count = ser.available(); // 读取到缓存区数据的字节数if (count > 0){int num;rclcpp::Time now = this->get_clock()->now(); // 获取时间戳std::vector read_buf(count);//这里定义无符号，是因为read函数的形参是无符号num = ser.read(&read_buf[0], count); // 读出缓存区缓存的数据，返回值为读到的数据字节数imu_fetch.FetchData(read_buf, num);sensor_msgs::msg::Imu imu_data;         //----------------imu data填充数据----------------imu_data.header.stamp = now;//imu_data.header.frame_id = "imu_link";imu_data.header.frame_id = "map";//加速度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.gyro_x ;imu_data.angular_velocity.y = imu_fetch.gyro_y ;imu_data.angular_velocity.z = imu_fetch.gyro_z ;//四元数imu_data.orientation.x = imu_fetch.quat_Q0;imu_data.orientation.y = imu_fetch.quat_Q1;imu_data.orientation.z = imu_fetch.quat_Q2;imu_data.orientation.w = imu_fetch.quat_Q3;pub_imu->publish(imu_data);  //向话题放数据/*Author: Mao Haiqing Time: 2023.7.6description: 读取IMU数据*/}}   };int main(int argc, char *argv[])
 {rclcpp::init(argc, argv);auto node = std::make_shared();//创建对应节点的共享指针对象rclcpp::spin(node);   //运行节点，并检测退出信号printf("线程退出n");rclcpp::shutdown();return 0;
 }
 

 transform.hpp

//transform.hpp
 #include 
 #include 
 #include 
 #include 
 class transform_imu
 {public:double acc_x=0;//加速度double acc_y=0;double acc_z=0;double gyro_x=0; //陀螺仪角速度double gyro_y=0;double gyro_z=0;double angle_r=0; //欧拉角double angle_p=0;double angle_y=0;double temp=0;  //imu温度计 double mag_x=0;  //磁力计                                                               double mag_y=0;double mag_z=0;double quat_Q0=0; //四元数double quat_Q1=0;double quat_Q2=0;double quat_Q3=0;public:transform_imu(){};  //默认构造函数~transform_imu(){}; //析构函数void FetchData(auto &data, int usLength) //获取数据
 {   int index = 0;unsigned char sum=0;//for(int i = 0;i < usLength;i++){printf("%x ",data[i]);} //打印数据，查看收到的数据是否正确while (usLength >= 40)//一个完整数据帧40字节[0xA4,0x03,0x08,0x23,数据35字节，校验位] {// 收到的数据开头4位：0xA4 帧头,0x03 读取,0x08 从8号寄存器开始,0x23 35个寄存器if (data[index] != 0xA4)//0xA4是协议头{index++;//指针(/索引)后移，继续找协议帧头usLength--;  continue;}//printf("找到数据包帧头，index是%d，uslength=%dn",index,usLength);//每一次读取的数据长度if(data[index + 1] != 0x03)   //判断第二个数据是否正确{printf("第2数据包内容不对，进入下个循环");usLength = usLength - 40;index += 40;continue;}   // printf("读imu数据n");if(data[index + 2] != 0x08)  //判断第3个数据是否正确{printf("第三数据包内容不对，进入下个循环");usLength = usLength - 40;index += 40;continue;}// printf("第08寄存器是数据n");if(data[index + 3] != 0x23) //判断第4个数据是否正确{printf("第四数据包内容不对，进入下个循环n");usLength = usLength - 40;index += 40;continue;}//printf("35个数据包n");  //0-39字节相加的值取低8位与第40位校验，参考GY-95T 九轴模块使用说明for(int i=index;i

cmake_minimum_required(VERSION 3.5)
 project(imu_get_cpp)# Default to C99
 if(NOT CMAKE_C_STANDARD)set(CMAKE_C_STANDARD 99)
 endif()# Default to C++14
 if(NOT CMAKE_CXX_STANDARD)set(CMAKE_CXX_STANDARD 14)
 endif()if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")add_compile_options(-Wall -Wextra -Wpedantic)
 endif()# find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(serial REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 #find_package(geometry_msgs)
 find_package(nav_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package( REQUIRED)add_executable(publisher_imu_node src/publisher_imu.cpp src/transform.hpp)
 ament_target_dependencies(publisher_imu_node rclcpp std_msgs)
 ament_target_dependencies(publisher_imu_node rclcpp serial)
 ament_target_dependencies(publisher_imu_node rclcpp sensor_msgs)
 ament_target_dependencies(publisher_imu_node rclcpp rosidl_default_generators)
 ament_target_dependencies(publisher_imu_node rclcpp nav_msgs)
 ament_target_dependencies(publisher_imu_node rclcpp tf2_ros)
 ament_target_dependencies(publisher_imu_node rclcpp tf2_msgs)
 ament_target_dependencies(publisher_imu_node rclcpp tf2_geometry_msgs)install(TARGETSpublisher_imu_nodeDESTINATION lib/${PROJECT_NAME})if(BUILD_TESTING)find_package(ament_lint_auto REQUIRED)# the following line skips the linter which checks for copyrights# uncomment the line when a copyright and license is not present in all source files#set(ament_cmake_copyright_FOUND TRUE)# the following line skips cpplint (only works in a git repo)# uncomment the line when this package is not in a git repo#set(ament_cmake_cpplint_FOUND TRUE)ament_lint_auto_find_test_dependencies()
 endif()ament_package()
 

 
 imu_get_cpp0.0.0TODO: Package descriptionmTODO: License declarationament_cmakerclcppstd_msgsserialsensor_msgsnav_msgstf2_rostf2_msgstf2_geometry_msgsgeometry_msgsament_lint_autoament_lint_commonament_cmake
 
 

cd ~/ros2_ws                                 //进入工作空间
 colcon build --packages-select imu_get_cpp   //编译source ~/ros2_ws/install/setup.bash          //source环境变量
 ros2 run imu_get_cpp publisher_imu_node      //运行节点

ros2 topic list    //查看话题/clicked_point
 /goal_pose
 /imu_data
 /initialpose
 /parameter_events
 /rosout
 /tf
 /tf_staticros2 topic echo /imu_data     //显示话题详细信息header:stamp:sec: 1688645970nanosec: 614702066frame_id: map
 orientation:x: 1.3034y: -0.2712z: -0.1872w: 0.9363
 orientation_covariance:
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 angular_velocity:x: -314.4512195121951y: 48.91463414634146z: 0.0
 angular_velocity_covariance:
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 linear_acceleration:x: -0.07177734375y: -3.9046875z: 9.168359375000001
 linear_acceleration_covariance:
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0
 - 0.0