ros2 c++实现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

目录结构：

imu_cpp.launch.py

#!/usr/bin/python3
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

    imu_node = Node(package='imu_get_cpp',
                    executable='publisher_imu_node',
                    name='publisher_imu_node',		
                    output='screen',                              
                    )
    return LaunchDescription([
        imu_node,
    ])

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 "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// 创建imu发布类继承自rclcpp::Node
{
private:
    std::string port;   //端口名
    int baudrate;       //波特率查看硬件说明
    transform_imu imu_fetch; //创建imu获取对象
    rclcpp::TimerBase::SharedPtr timer_;   //创建定时器
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu; // 创建发布者
public:
    publisher_imu_node()
        : Node("publisher_imu_node")  //构造函数
    {
        port = "/dev/ttyUSB0";  // 根据自己的主机修改串口号
        baudrate = 115200;      // 波特率，imu在波特率115200返回数据时间大概是1ms,9600下大概是10ms
        try
        {
            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(), "imu串口初始化完成！");
            unsigned char cmd_buffer[5] ={0xA4,0x03,0x08,0x23,0xD2}; //JY-95T启用35个寄存器 需要发送的串口包命令
            ser.write(cmd_buffer,5);  //JY-95T可以根据命令选择工作方式
            printf("imu发出命令: %x %x %x %x %x\n",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<sensor_msgs::msg::Imu>("/imu_data", 20);//创建了话题 /imu_data
        
        // 创建定时器，100ms为周期，定时发布，这个时间设置超过200毫秒发布的数据卡顿
        timer_ = this->create_wall_timer(100ms, std::bind(&publisher_imu_node::timer_callback, this));
        printf("imu发布线程工作中\n");
    }

public:
    void timer_callback()   //定时器的回调函数，每20毫秒回调一次
    {
        int count = ser.available(); // 读取到缓存区数据的字节数
        
        if (count > 0)
        {
          
            int num;
            rclcpp::Time now = this->get_clock()->now(); // 获取时间戳

            std::vector<unsigned char> 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.6
            description: 读取IMU数据
            */
          
        }
    }   
   
   
};



int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<publisher_imu_node>();//创建对应节点的共享指针对象
    rclcpp::spin(node);   //运行节点，并检测退出信号
    printf("imu线程退出\n");
    rclcpp::shutdown();
    return 0;
}

transform.hpp

#include <string>
#include <ctype.h>
#include <float.h>
#include <math.h>
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) //获取数据 
    void FetchData(std::vector<unsigned char> &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=%d\n",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<index+40;i++) //前39为求和
          {
            sum=0+data[i]; 
          }
            
         if(sum != data[index+39]) //判断前39个数据的和是否与第40位相等
        {
            //printf("校验和不对，进入下个循环\n");
            usLength = usLength - 40;
            index += 40;
            continue;
        }
         // printf("校验和通过，数据没有问题\n"); 


        /*
        根据模块使用说明可知，一个完整数据帧40字节[0xA4,0x03,0x08,0x23,数据35字节，校验位]
        1.开头4位：0xA4 帧头,0x03 读取,0x08 数据位从8号寄存器开始,0x23 35个寄存器
        2.加速度位：4-9   
        3.陀螺仪位：10-15
        4.欧拉角：16-21
        5.磁场校准：22
        6.器件温度：23-24
        5.磁场：25-30
        6.四元数：31-38
        数据解析一定注意2个字节拼在一起用short类型，然后在转换成double类型，总体加小括号在作除法，
        否则short类型作除法会被截断降低精度，表现在图形显示界面会卡，数据不流畅。拼好的数据单位参考
        使用手册。
        */

        // 加速度
            acc_x = ((double)((short)(data[index + 5]<<8 | data[index + 4])))/2048 * 9.8;  //单位m/s^2
            acc_y = ((double)((short)(data[index + 7]<<8 | data[index + 6])))/2048 * 9.8;
            acc_z = ((double)((short)(data[index + 9]<<8 | data[index + 8])))/2048 * 9.8;

        // 陀螺仪角速度
            gyro_x = ((double)((short)(data[index + 11]<<8 | data[index + 10])))/16.4*3.1415926 / 180.0; //单位rad/sec
            gyro_y = ((double)((short)(data[index + 13]<<8 | data[index + 12])))/16.4*3.1415926 / 180.0;  
            gyro_z = ((double)((short)(data[index + 15]<<8 | data[index + 14])))/16.4*3.1415926 / 180.0;  
        
        //欧拉角
            // angle_r = ((double)((short)(data[index + 17]<<8 | data[index + 16])))/100 ; 
            // angle_p = ((double)((short)(data[index + 19]<<8 | data[index + 18])))/100 ; 
            // angle_y = ((double)((short)(data[index + 21]<<8 | data[index + 20])))/100 ; 
        
        //imu温度
            temp = ((double)((short)(data[index + 24]<<8 | data[index + 23])))/100 ; //单位摄氏度
        //磁力计
            // mag_x = ((double)((short)(data[index + 26]<<8 | data[index + 25]))); 
            // mag_y = ((double)((short)(data[index + 28]<<8 | data[index + 27]))); 
            // mag_z = ((double)((short)(data[index + 30]<<8 | data[index + 29])));
            

        //四元数  
            quat_Q0 = ((double)((short)(data[index + 32]<<8 | data[index + 31])))/10000 ; //无单位
            quat_Q1 = ((double)((short)(data[index + 34]<<8 | data[index + 33])))/10000 ; 
            quat_Q2 = ((double)((short)(data[index + 36]<<8 | data[index + 35])))/10000 ; 
            quat_Q3 = ((double)((short)(data[index + 38]<<8 | data[index + 37])))/10000 ; 

            //JY-95T imu传感器9轴，数据解析完成，具体用什么数据可以根据实际需要选用
          
            //调试结束后可以屏蔽以下printf
            //printf("加速度x y z: %lf %lf %lf\n",acc_x,acc_y,acc_z);
            //printf("角速度x y z: %lf %lf %lf\n",gyro_x,gyro_y,gyro_z);
            //printf("欧拉角r p y: %lf %lf %lf %lf\n",angle_r,angle_p,angle_y);
            //printf("imu温度计 %lf\n",temp);
            //printf("磁力计x y z: %lf %lf %lf\n",mag_x, mag_y,mag_z);
            //printf("四元数Q0 Q1 Q2 Q3: %lf %lf %lf %lf\n",quat_Q0,quat_Q1,quat_Q2,quat_Q3);
            /*
            Author: Mao Haiqing 
            Time: 2023.7.6
            description: 读取IMU数据
            */
            usLength = usLength - 40;
            index += 40;
    
     }
}

};

CMakeLists.txt

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(<dependency> 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(TARGETS
  publisher_imu_node
  DESTINATION lib/${PROJECT_NAME})
install(
  DIRECTORY launch
  DESTINATION share/${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()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>imu_get_cpp</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="330406269@qq.com">m</maintainer>
  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  <depend>serial</depend>
  <depend>sensor_msgs</depend>
  <depend>nav_msgs</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_msgs</depend>
  <depend>tf2_geometry_msgs</depend>
  <depend>geometry_msgs</depend>


  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

编译
colcon build --packages-select imu_get_cpp
运行
ros2 launch imu_get_cpp imu_cpp.launch.py
查看话题
ros2 topic list
/clicked_point
/goal_pose
/imu_data
/initialpose
/parameter_events
/rosout
/tf
/tf_static

ros2 topic echo /imu_data

header:
stamp:
sec: 1693478860
nanosec: 800160028
frame_id: imu_link
orientation:
x: 0.0866
y: -0.0067
z: 0.0078
w: -1.636
orientation_covariance:

• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
  angular_velocity:
  x: 0.0010642251355013552
  y: 0.0010642251355013552
  z: 0.0010642251355013552
  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.08134765625000001
  y: -0.09091796875000001
  z: 9.866992187500001
  linear_acceleration_covariance:
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0
• 0.0

---

数据正常，运行成功。

[毛哥成山轮胎机油保养]

@毛哥成山轮胎机油保养 我现在有一个疑问，2种运行方式，为什么程序运行的不一样呢，终端返回的信息也不一样 ？
ros2 launch imu_get_cpp imu_cpp.launch.py

[INFO] [launch]: All log files can be found below /home/m/.ros/log/2023-08-31-18-56-23-189690-ubun-13754
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [publisher_imu_node-1]: process started with pid [13756]
[publisher_imu_node-1] [INFO] [1693479383.314753546] [publisher_imu_node]: imu串口初始化完成！
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[publisher_imu_node-1] [INFO] [1693479445.030912184] [rclcpp]: signal_handler(signal_value=2)
[publisher_imu_node-1] imu发出命令: a4 3 8 23 d2
[publisher_imu_node-1] imu发布线程工作中
[publisher_imu_node-1] imu线程退出

/home/m/ros2_ws/install/imu_get_cpp/lib/imu_get_cpp/publisher_imu_node

[INFO] [1693479448.843189189] [publisher_imu_node]: imu串口初始化完成！
imu发出命令: a4 3 8 23 d2
imu发布线程工作中
^C^C[INFO] [1693479476.254140522] [rclcpp]: signal_handler(signal_value=2)
imu线程退出

按理说这2种运行方式都是一样的，第一种是ros2的标准运行命令，第二种是按c++的运行命令，为什么程序运行的方式不一样呢，有没有懂的大佬给解释一下，非常感谢