鱼香ROS社区
    • 版块
    • 最新
    • 未解决
    • 已解决
    • 群组
    • 注册
    • 登录
    紧急通知:禁止一切关于政治&VPN翻墙等话题,发现相关帖子会立马删除封号
    提问前必看的发帖注意事项: 社区问答规则(小鱼个人)更新 | 高质量帖子发布指南

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

    已定时 已固定 已锁定 已移动
    移动平台分享
    imu驱动 ros2
    1
    2
    641
    正在加载更多帖子
    • 从旧到新
    • 从新到旧
    • 最多赞同
    回复
    • 在新帖中回复
    登录后回复
    此主题已被删除。只有拥有主题管理权限的用户可以查看。
    • 毛哥成山轮胎机油保养毛
      毛哥成山轮胎机油保养 活跃VIP
      最后由 编辑

      2023-07-06 18-05-52屏幕截图.png
      2023-07-06 18-01-45屏幕截图.png
      2023-07-06 18-02-29屏幕截图.png
      2023-07-06 18-04-00屏幕截图.png
      2023-07-06 18-04-00屏幕截图.png
      2023-07-06 18-04-32屏幕截图.png
      2023-07-06 18-04-50屏幕截图.png
      2023-07-06 18-05-12屏幕截图.png
      2023-07-06 18-06-10屏幕截图.png
      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

      目录结构:
      2023-08-31 18-32-37屏幕截图.png
      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

      数据正常,运行成功。

      毛哥成山轮胎机油保养毛 1 条回复 最后回复 回复 引用 0
      • 毛哥成山轮胎机油保养毛
        毛哥成山轮胎机油保养 活跃VIP @毛哥成山轮胎机油保养
        最后由 编辑

        @毛哥成山轮胎机油保养 我现在有一个疑问,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++的运行命令,为什么程序运行的方式不一样呢,有没有懂的大佬给解释一下,非常感谢

        1 条回复 最后回复 回复 引用 0
        • 毛哥成山轮胎机油保养毛 毛哥成山轮胎机油保养 在 中 引用了 这个主题
        • 毛哥成山轮胎机油保养毛 毛哥成山轮胎机油保养 在 中 引用了 这个主题
        • 第一个帖子
          最后一个帖子
        皖ICP备16016415号-7
        Powered by NodeBB | 鱼香ROS