ROS2 Foxy下，六轴IMU串口数据解析与Rviz2可视化实战（避坑串口驱动与协议）

ROS2 Foxy环境下六轴IMU数据解析与Rviz2可视化全流程指南

1. 硬件准备与串口配置

在开始之前，我们需要确保硬件连接正确。将六轴IMU通过USB转TTL模块连接到计算机的USB端口。大多数现代IMU模块都采用标准串口通信协议，常见的有0x55协议头和JY901协议等。

关键检查点：

• 确认IMU供电正常（通常3.3V或5V）
• 检查USB转串口模块驱动是否兼容
• 确认波特率设置与IMU出厂设置一致（常见有9600、115200等）

在Ubuntu 20.04中，串口设备通常会被识别为/dev/ttyUSB*或/dev/ttyACM*。可以通过以下命令查看：

ls /dev/ttyUSB*

如果没有任何输出，可能需要检查驱动安装情况。对于CH340/CH341等常见USB转串口芯片，可以使用以下命令安装驱动：

sudo apt-get install build-essential sudo apt-get install linux-headers-$(uname -r) sudo apt-get install git git clone https://github.com/juliagoda/CH341SER.git cd CH341SER make sudo make install

安装完成后，需要给串口设备赋予读写权限：

sudo chmod 777 /dev/ttyUSB0

注意：每次重新插拔USB设备后，可能需要重新执行权限设置命令。为避免每次手动设置，可以创建udev规则。

2. ROS2 Foxy环境搭建

确保已正确安装ROS2 Foxy版本。如果尚未安装，可以按照以下步骤进行：

sudo apt update sudo apt install curl gnupg2 lsb-release curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - sudo sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2.list' sudo apt update sudo apt install ros-foxy-desktop

安装完成后，初始化工作空间：

mkdir -p ~/imu_ws/src cd ~/imu_ws colcon build

3. 串口通信库安装与配置

ROS2提供了serial库用于串口通信，但需要单独安装：

sudo apt install ros-foxy-serial-driver

此外，我们还需要安装一些必要的依赖：

sudo apt install ros-foxy-sensor-msgs ros-foxy-tf2 ros-foxy-tf2-ros

创建一个新的ROS2包来处理IMU数据：

cd ~/imu_ws/src ros2 pkg create --build-type ament_cmake imu_processing cd imu_processing

4. IMU数据协议解析

六轴IMU通常采用二进制协议传输数据，常见帧结构如下：

下面是一个典型的C++解析类实现，保存为include/imu_processing/imu_parser.hpp：

#pragma once #include <vector> #include <cmath> class IMUParser { public: struct IMUData { double accel_x = 0.0; double accel_y = 0.0; double accel_z = 0.0; double gyro_x = 0.0; double gyro_y = 0.0; double gyro_z = 0.0; }; void parseRawData(const std::vector<uint8_t>& raw_data) { size_t index = 0; while(index + 10 < raw_data.size()) { if(raw_data[index] != 0x55) { index++; continue; } switch(raw_data[index+1]) { case 0x51: // 加速度数据 current_data_.accel_x = static_cast<int16_t>((raw_data[index+3]<<8)|raw_data[index+2]) / 32768.0 * 16 * 9.8; current_data_.accel_y = static_cast<int16_t>((raw_data[index+5]<<8)|raw_data[index+4]) / 32768.0 * 16 * 9.8; current_data_.accel_z = static_cast<int16_t>((raw_data[index+7]<<8)|raw_data[index+6]) / 32768.0 * 16 * 9.8; break; case 0x52: // 角速度数据 current_data_.gyro_x = static_cast<int16_t>((raw_data[index+3]<<8)|raw_data[index+2]) / 32768.0 * 2000 * M_PI / 180.0; current_data_.gyro_y = static_cast<int16_t>((raw_data[index+5]<<8)|raw_data[index+4]) / 32768.0 * 2000 * M_PI / 180.0; current_data_.gyro_z = static_cast<int16_t>((raw_data[index+7]<<8)|raw_data[index+6]) / 32768.0 * 2000 * M_PI / 180.0; break; } index += 11; } } const IMUData& getData() const { return current_data_; } private: IMUData current_data_; };

5. ROS2节点实现

创建IMU数据发布节点，保存为src/imu_publisher.cpp：

#include "imu_processing/imu_parser.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/imu.hpp" #include "serial/serial.h" class IMUPublisher : public rclcpp::Node { public: IMUPublisher() : Node("imu_publisher") { // 参数声明 declare_parameter("port", "/dev/ttyUSB0"); declare_parameter("baudrate", 115200); declare_parameter("frame_id", "imu_link"); // 获取参数 port_ = get_parameter("port").as_string(); baudrate_ = get_parameter("baudrate").as_int(); frame_id_ = get_parameter("frame_id").as_string(); // 初始化串口 try { serial_.setPort(port_); serial_.setBaudrate(baudrate_); serial::Timeout timeout = serial::Timeout::simpleTimeout(1000); serial_.setTimeout(timeout); serial_.open(); } catch (const std::exception& e) { RCLCPP_FATAL(get_logger(), "Failed to open serial port: %s", e.what()); rclcpp::shutdown(); } // 创建发布者 publisher_ = create_publisher<sensor_msgs::msg::Imu>("imu/data", 10); // 创建定时器 timer_ = create_wall_timer( std::chrono::milliseconds(10), std::bind(&IMUPublisher::timerCallback, this)); } private: void timerCallback() { if (serial_.available()) { std::vector<uint8_t> raw_data; serial_.read(raw_data, serial_.available()); parser_.parseRawData(raw_data); auto imu_data = parser_.getData(); auto message = sensor_msgs::msg::Imu(); message.header.stamp = now(); message.header.frame_id = frame_id_; // 填充加速度数据 message.linear_acceleration.x = imu_data.accel_x; message.linear_acceleration.y = imu_data.accel_y; message.linear_acceleration.z = imu_data.accel_z; // 填充角速度数据 message.angular_velocity.x = imu_data.gyro_x; message.angular_velocity.y = imu_data.gyro_y; message.angular_velocity.z = imu_data.gyro_z; // 发布消息 publisher_->publish(message); } } serial::Serial serial_; IMUParser parser_; rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_; rclcpp::TimerBase::SharedPtr timer_; std::string port_; int baudrate_; std::string frame_id_; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<IMUPublisher>()); rclcpp::shutdown(); return 0; }

6. 编译配置

更新CMakeLists.txt文件：

cmake_minimum_required(VERSION 3.5) project(imu_processing) # 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(sensor_msgs REQUIRED) find_package(serial REQUIRED) # Add include directory include_directories(include) # Create executable add_executable(imu_publisher src/imu_publisher.cpp) target_include_directories(imu_publisher PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<INSTALL_INTERFACE:include>) ament_target_dependencies(imu_publisher rclcpp sensor_msgs serial) # Install targets install(TARGETS imu_publisher DESTINATION lib/${PROJECT_NAME}) # Install include files install(DIRECTORY include/ DESTINATION include) 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_processing</name> <version>0.0.0</version> <description>ROS2 package for IMU data processing</description> <maintainer email="user@example.com">Your Name</maintainer> <license>Apache License 2.0</license> <buildtool_depend>ament_cmake</buildtool_depend> <depend>rclcpp</depend> <depend>sensor_msgs</depend> <depend>serial</depend> <test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_common</test_depend> </package>

7. 构建与运行

在工作空间根目录下执行构建命令：

cd ~/imu_ws colcon build --packages-select imu_processing

构建成功后，设置环境变量并运行节点：

source install/setup.bash ros2 run imu_processing imu_publisher

可以通过以下命令查看发布的IMU数据：

ros2 topic echo /imu/data

8. Rviz2可视化配置

安装必要的可视化插件：

sudo apt install ros-foxy-rviz2 ros-foxy-rviz-imu-plugin

启动Rviz2：

rviz2

在Rviz2中进行如下配置：

1. 点击"Add"按钮
2. 选择"By topic"选项卡
3. 找到/imu/data话题下的"IMU"显示类型并添加
4. 在"Global Options"中设置"Fixed Frame"为imu_link（与代码中设置的frame_id一致）

常见问题排查：

1. 串口无法打开

   • 检查设备权限：ls -l /dev/ttyUSB*
   • 确保没有其他程序占用串口
   • 尝试不同的波特率

2. 数据解析错误

   • 确认IMU协议与代码解析逻辑一致
   • 打印原始数据验证格式
   • 检查字节序处理是否正确

3. Rviz2中无显示

   • 确认话题名称匹配
   • 检查frame_id设置
   • 确保数据正在发布（使用ros2 topic hz /imu/data检查频率）

9. 进阶优化

数据时间同步处理：

// 在IMUParser类中添加时间戳处理 struct IMUData { // ...原有字段... rclcpp::Time timestamp; }; // 在parseRawData方法中更新时间戳 current_data_.timestamp = rclcpp::Clock().now();

添加协方差信息：

// 在发布消息前添加协方差估计 message.orientation_covariance = {0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01}; message.angular_velocity_covariance = {0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02}; message.linear_acceleration_covariance = {0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04};

使用TF2发布坐标系变换：

#include "tf2_ros/transform_broadcaster.h" // 在IMUPublisher类中添加成员 std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_; // 在构造函数中初始化 tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this); // 在timerCallback中添加TF发布 geometry_msgs::msg::TransformStamped transform; transform.header.stamp = now(); transform.header.frame_id = "base_link"; transform.child_frame_id = frame_id_; transform.transform.rotation.w = 1.0; // 无旋转 tf_broadcaster_->sendTransform(transform);