ROS开发专栏---IMU 数据读取实验实现---适配Ubuntu 22.04

🎬 渡水无言 ：个人主页渡水无言

❄专栏传送门 ：《linux专栏》《嵌入式linux驱动开发》《linux系统移植专栏》

❄专栏传送门 ：《freertos专栏》《STM32 HAL库专栏》《linux裸机开发专栏》

❄专栏传送门 ：《产品测评专栏》《Ai智能体专栏）《ROS开发专栏》

⭐️流水不争先，争的是滔滔不绝

📚博主简介：第二十届中国研究生电子设计竞赛全国二等奖 |国家奖学金 | 省级三好学生

| 省级优秀毕业生获得者 | csdn新星杯TOP18 | 半导纵横专栏博主 | 211在读研究生

在这里主要分享自己学习的linux嵌入式领域知识；有分享错误或者不足的地方欢迎大佬指导，也欢迎各位大佬互相三连

前言

[一、IMU 基础知识点](#一、IMU 基础知识点)

二、IMU数据获取的原理

[三、IMU 数据读取与打印实验](#三、IMU 数据读取与打印实验)

3.1、创建功能包

[3.2、编写节点代码imu_data.cpp](#3.2、编写节点代码imu_data.cpp)

3.3、设置编译规则CMakeLists.txt

[3.4、配置 package.xml](#3.4、配置 package.xml)

四、编译与运行

总结

前言

上一期博客我们使用激光雷达实现了机器人的自动避障，本期博客我们来学习 ROS2 中另一种核心传感器 ------IMU（惯性测量单元）。

IMU 用于测量机器人的空间姿态吗，可以输出机器人的加速度、角速度、姿态角，是实现里程计、平衡、导航、航向锁定的关键。

一、IMU 基础知识点

ROS2 中 IMU 常用话题

/imu/data_raw：原始加速度、陀螺仪数据。

/imu/data：融合后带姿态（四元数）的数据。

/imu/mag：磁力计数据。

IMU 消息类型

sensor_msgs/msg/Imu

包含：

姿态四元数 orientation

角速度 angular_velocity

线加速度 linear_acceleration
姿态表示

四元数：x、y、z、w

转换成下边的欧拉角：

欧拉角（角度）：

roll 翻滚角

机器人左右倾斜，类似飞机左右侧倾。

如下图所示：

pitch 俯仰角

机器人前后仰俯，类似飞机抬头、低头。

如下图所示：

yaw 偏航角（航向角，最常用）

机器人水平旋转、改变朝向，类似汽车左转、右转。

x y z w 是机器看懂的姿态 → 转成 → yaw 是人看懂的角度！

二、IMU数据获取的原理

IMU数据的获取是通过订阅IMU节点发布的话题，从话题中获取IMU节点发出的消息包来实现的。如下图所示：

三、IMU 数据读取与打印实验

3.1、创建功能包

首先还是先创建功能包，输入以下代码：

复制代码

cd ~/ros2_zice/src
ros2 pkg create imu_pkg

3.2、编写节点代码imu_data.cpp

首先还是在imu_pkg功能包里的src文件夹里添加imu_data.cpp节点文件，并编写节点代码，如下所示：

复制代码

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

//定义了一个全局变量node,用于在回调函数中打印日志
std::shared_ptr<rclcpp::Node> node;

// IMU数据回调函数,IMU每次发布数据时都会调用这个函数
void IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
    // 1. 读取IMU四元数
    tf2::Quaternion tf_quat;
    tf_quat.setX(msg->orientation.x);
    tf_quat.setY(msg->orientation.y);
    tf_quat.setZ(msg->orientation.z);
    tf_quat.setW(msg->orientation.w);

    // 2. 转为旋转矩阵
    tf2::Matrix3x3 mat(tf_quat);

    // 3. 转欧拉角（弧度）
    double roll, pitch, yaw;
    mat.getRPY(roll, pitch, yaw);

    // 转为角度
    roll  = roll  * 180 / M_PI;
    pitch = pitch * 180 / M_PI;
    yaw   = yaw   * 180 / M_PI;

    // 打印
    RCLCPP_INFO(node->get_logger(), "roll=%.0f  pitch=%.0f  yaw=%.0f", roll, pitch, yaw);
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    node = std::make_shared<rclcpp::Node>("imu_data_node");
    // 订阅IMU数据,主题名为/imu/data,队列长度为10,回调函数为IMUCallback
    auto sub = node->create_subscription<sensor_msgs::msg::Imu>("/imu/data", 10, IMUCallback);

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

3.3、设置编译规则CMakeLists.txt

输入以下代码：

cpp 复制代码

cmake_minimum_required(VERSION 3.8)
project(imu_pkg)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(tf2 REQUIRED)

# 编译 IMU 数据读取节点
add_executable(imu_data src/imu_data.cpp)
ament_target_dependencies(imu_data
  rclcpp
  sensor_msgs
  tf2
)

# 安装节点
install(TARGETS
  imu_data
  DESTINATION lib/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  set(ament_cmake_copyright_FOUND TRUE)
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

3.4、配置 package.xml

添加以下依赖：

cpp 复制代码

<depend>rclcpp</depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>

最终代码如下：

cpp 复制代码

<?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_pkg</name>
  <version>0.0.0</version>
  <description>IMU data read</description>
  <maintainer email="todo@todo.todo">todo</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <!-- 依赖就加在这里！↓↓↓ -->
  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
  <depend>tf2</depend>

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

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

四、编译与运行

编译

cpp 复制代码

cd ~/ros2_zice
colcon build
source install/setup.bash

启动仿真环境

新开终端：

cpp 复制代码

source ~/ros2_zice/install/setup.bash
ros2 launch wpr_simulation2 wpb_simple.launch.py

运行 IMU 数据读取节点

新开终端

cpp 复制代码

source ~/ros2_zice/install/setup.bash
ros2 run imu_pkg imu_data

终端实时打印 ：roll、pitch、yaw 三个姿态角

此时我们可以改变机器人的位置，点击哪个旋转按钮，此时可以看到机器人周围出现了三个大圆环，

此时我们可以点击蓝色圆环进行旋转，我们可以看到朝向角进行了改变

我们也可以点击绿色圆环进行旋转，这样俯仰角就有变化了

我们也可以点击红色圆环进行旋转，这样翻滚角也有了变化。

总结

本期博客我们完成了ROS2 中 IMU 数据的基本读取与解析方法，并进行了读取实验，终端实时打印：roll、pitch、yaw 三个姿态角，移动机器人时，角度会实时变化。

ROS开发专栏---IMU 数据读取实验实现---适配Ubuntu 22.04

前言

一、IMU 基础知识点

二、IMU数据获取的原理

三、IMU 数据读取与打印实验

3.1、创建功能包

3.2、编写 节点代码imu_data.cpp

3.3、设置编译规则CMakeLists.txt

3.4、配置 package.xml

四、编译与运行

总结

3.2、编写节点代码imu_data.cpp