通过本案例可以掌握：

如何使用 C++ 编写一个 ROS 设备驱动节点；
如何调试 ROS 发布频率、时间戳和消息内容；
如何结合 rosbag 与 rqt_plot 分析传感器数据的稳定性；
为后续多传感器时间同步与融合（如 EKF）打下基础。

多传感器融合中的 ROS 驱动开发实战案例

在自动驾驶系统中，IMU、GNSS 和雷达等传感器是实现环境感知和车辆定位的关键。本文以 ROS 框架为基础，讲解如何编写一个简单的传感器驱动节点，实现数据采集 → 解析 → 发布 → 可视化，并附带常见调试命令。

1. 项目结构

my_sensor_driver/
├── launch/
│   └── sensor_driver.launch
├── src/
│   └── imu_driver_node.cpp
├── include/
│   └── imu_driver.hpp
├── config/
│   └── imu_config.yaml
├── CMakeLists.txt
├── package.xml

my_sensor_driver： ROS package 根目录，所有内容都放在这里

launch/sensor_driver.launch：

ROS 的启动脚本，用 XML 格式描述：

启动哪个节点（node）
使用哪些参数文件（如 config/imu_config.yaml）
是否输出日志到屏幕等
用法：roslaunch my_sensor_driver sensor_driver.launch
示例：

<launch>
<!-- 加载 IMU 参数配置 -->
<rosparam file="$(find my_sensor_driver)/config/imu_config.yaml" command="load"/>

<!-- 启动 IMU 驱动节点 -->
<node name="imu_driver" pkg="my_sensor_driver" type="imu_driver_node" output="screen"/>
</launch>

src/imu_driver_node.cpp
C++ 源码文件，实现如下功能：
- 初始化 ROS 节点
- 串口读 IMU 数据
- 解析并发布为 /imu/data ROS 消息
include/imu_driver.hpp：头文件，通常用于：
- 声明类 ImuDriver、函数、结构体等
- 和 cpp 分离，便于模块化、复用
- 如果项目简单也可以省略，用于“规范化”大型项目
config/imu_config.yaml：参数配置文件，用于将 IMU 驱动的参数（如串口号、波特率、话题名等）外部化管理

示例：

port: "/dev/ttyUSB0"
baudrate: 115200
frame_id: "imu_link"

在 sensor_driver.launch 中通过 rosparam 加载：

<rosparam file="$(find my_sensor_driver)/config/imu_config.yaml" command="load"/>

CMakeLists.txt:ROS 的构建脚本:
作用：
- 编译 src/imu_driver_node.cpp → 可执行文件 imu_driver_node
- 指定依赖（如 sensor_msgs、roscpp）
示例：

cmake_minimum_required(VERSION 3.0.2)
project(my_sensor_driver)

## 编译类型设置
add_compile_options(-std=c++11)

## 查找依赖的包
find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
  std_msgs
)

## 生成消息的路径设置
catkin_package()

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

## 编译可执行文件
add_executable(imu_driver_node src/imu_driver_node.cpp)
target_link_libraries(imu_driver_node ${catkin_LIBRARIES})

package.xml: ROS 的包描述文件

作用：
- 告诉 ROS 这个包的名称、依赖库、作者信息等
- 配合 catkin_make 使用

示例：

<?xml version="1.0"?>
<package format="2">
  <name>my_sensor_driver</name>
  <version>0.0.1</version>
  <description>Minimal ROS sensor driver package</description>

  <maintainer email="your_email@example.com">Your Name</maintainer>
  <license>BSD</license>

  <buildtool_depend>catkin</buildtool_depend>

  <depend>roscpp</depend>
  <depend>sensor_msgs</depend>
  <depend>std_msgs</depend>

  <export>
  </export>
</package>

使用 catkin_make打包与运行流程:

cd ~/catkin_ws
catkin_make
source devel/setup.bash
roslaunch my_sensor_driver sensor_driver.launch

TODO: 模拟 IMU 串口发送脚本（mock_imu_sender.py）模拟 GNSS ROS 节点（mock_gnss_node.py） EKF launch 文件

2. 驱动逻辑核心（src/imu_driver_node.cpp部分代码示例）

实现读取串口数据并发布为 /imu/data 的逻辑

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <serial/serial.h>
#include <string>
#include <sstream>

// 模拟解析函数：从原始字符串中提取四元数（实际应依据具体协议实现）
void parseIMUData(const std::string& raw, sensor_msgs::Imu& imu_msg) {
    // 示例数据: "0.0,0.0,0.0,1.0"
    std::stringstream ss(raw);
    std::string item;
    float q[4];
    int i = 0;

    while (std::getline(ss, item, ',') && i < 4) {
        q[i++] = std::stof(item);
    }

    imu_msg.orientation.x = q[0];
    imu_msg.orientation.y = q[1];
    imu_msg.orientation.z = q[2];
    imu_msg.orientation.w = q[3];
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "imu_driver_node");
    ros::NodeHandle nh;
    ros::NodeHandle pnh("~");

    std::string port;
    int baudrate;
    std::string frame_id;

    // 获取参数
    pnh.param<std::string>("port", port, "/dev/ttyUSB0");
    pnh.param<int>("baudrate", baudrate, 115200);
    pnh.param<std::string>("frame_id", frame_id, "imu_link");

    // 初始化串口
    serial::Serial ser;
    try {
        ser.setPort(port);
        ser.setBaudrate(baudrate);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        ser.open();
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM("Unable to open port " << port);
        return -1;
    }

    if (!ser.isOpen()) {
        ROS_ERROR_STREAM("Serial port not opened.");
        return -1;
    }

    ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("/imu/data", 50);

    ROS_INFO_STREAM("IMU Driver Started on " << port << " at " << baudrate << " baudrate.");

    ros::Rate loop_rate(50);
    while (ros::ok()) {
        if (ser.available()) {
            std::string raw_data = ser.readline();
            sensor_msgs::Imu imu_msg;
            imu_msg.header.stamp = ros::Time::now();
            imu_msg.header.frame_id = frame_id;

            parseIMUData(raw_data, imu_msg);
            imu_pub.publish(imu_msg);
        }
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

🚀 3. Launch 文件示例

<launch>
  <node pkg="my_sensor_driver" type="imu_driver_node" name="imu_driver" output="screen" />
</launch>

4. 常用 ROS 调试命令（推荐操作顺序）

操作目的	常用命令
查看 topic 发布频率	`rostopic hz /imu/data`
查看发布数据内容	`rostopic echo /imu/data`
查看所有活跃节点	`rosnode list`
检查节点状态	`rosnode info /imu_driver`
可视化数据	`rqt_plot /imu/data.orientation.x`
录制调试日志	`rosbag record -O imu_log.bag /imu/data`
播放 rosbag 日志	`rosbag play imu_log.bag`

5. 延迟与频率验证技巧

# 查看延迟（是否 timestamp 稳定）
rostopic echo -n 5 /imu/data | grep "stamp"

# 绘制数据间隔时间
rqt_plot /imu/data.header.stamp

6. 总结

下一步是整合 GNSS 数据，并使用 message_filters::ApproximateTime 实现 IMU + GNSS 时间同步。

🛰️ GNSS + IMU 融合定位（基于 ROS + EKF）

接下来这部分将介绍如何基于 ROS 框架将 GNSS 与 IMU 数据进行融合，利用 robot_localization 包中的 EKF 节点实现车辆位置估计，提高定位的鲁棒性和精度。

1. 使用包依赖

sudo apt install ros-<distro>-robot-localization

2. 数据准备

输入数据：

/imu/data：IMU 原始数据（方向、加速度）
/gps/fix：GNSS 数据（sensor_msgs/NavSatFix）

可选增强数据：

/gps/vel：GNSS 速度信息
/odometry/wheel：轮速计（用于补偿 GNSS 信号丢失）

3. EKF 节点配置文件（ekf.yaml）

frequency: 30
sensor_timeout: 0.1

two_d_mode: true
publish_tf: true

map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom

imu0: /imu/data
imu0_config: [false, false, false,
              true, true, false,
              false, false, false,
              false, false, false,
              false, false, false]
imu0_differential: false
imu0_remove_gravitational_acceleration: true

gps0: /gps/fix
gps0_config: [true, true, false,
              false, false, false,
              false, false, false,
              false, false, false,
              false, false, false]
gps0_differential: false
use_control: false

4. 启动文件（ekf_fusion.launch）

<launch>
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" output="screen">
    <param name="~config" value="$(find your_package)/config/ekf.yaml" />
  </node>
</launch>

5. 可视化与验证

6. 问题排查建议

问题	原因	解决建议
融合输出跳变 / 无输出	时间戳不一致	确保所有话题时间同步（使用 rosbag + `--clock`）
EKF 输出延迟大	频率设置太低或topic不活跃	增加 `frequency`，确认输入话题活跃
数据漂移严重	IMU 重力未剔除	设置 `imu0_remove_gravitational_acceleration: true`
融合失败报错	配置字段顺序错误	检查每个 `*_config` 数组长度为15，顺序严格

7. 总结

通过本案例，我们可以掌握：

使用 robot_localization 进行 GNSS + IMU 融合定位；
如何配置 EKF 参数以适配不同传感器特性；
如何通过 rviz / rqt_plot 进行融合结果验证；
以及时间同步、漂移修正等常见问题的处理方法。

下一步我们可以尝试加入轮速计、视觉里程计等，进行三传感器融合。

如何使用 C++ 编写一个 ROS 节点

多传感器融合中的 ROS 驱动开发实战案例

1. 项目结构

2. 驱动逻辑核心（src/imu_driver_node.cpp部分代码示例）

🚀 3. Launch 文件示例

4. 常用 ROS 调试命令（推荐操作顺序）

5. 延迟与频率验证技巧

6. 总结

🛰️ GNSS + IMU 融合定位（基于 ROS + EKF）

1. 使用包依赖

2. 数据准备

输入数据：

可选增强数据：

3. EKF 节点配置文件（ekf.yaml）

4. 启动文件（ekf_fusion.launch）

5. 可视化与验证

推荐指令：

6. 问题排查建议

7. 总结

多传感器融合中的 ROS 驱动开发实战案例#

1. 项目结构#

2. 驱动逻辑核心（src/imu_driver_node.cpp部分代码示例）#

🚀 3. Launch 文件示例#

4. 常用 ROS 调试命令（推荐操作顺序）#

5. 延迟与频率验证技巧#

6. 总结#

🛰️ GNSS + IMU 融合定位（基于 ROS + EKF）#

1. 使用包依赖#

2. 数据准备#

输入数据：#

可选增强数据：#

3. EKF 节点配置文件（ekf.yaml）#

4. 启动文件（ekf_fusion.launch）#

5. 可视化与验证#

推荐指令：#

6. 问题排查建议#

7. 总结#

多传感器融合中的 ROS 驱动开发实战案例

1. 项目结构

2. 驱动逻辑核心（src/imu_driver_node.cpp部分代码示例）

🚀 3. Launch 文件示例

4. 常用 ROS 调试命令（推荐操作顺序）

5. 延迟与频率验证技巧

6. 总结

🛰️ GNSS + IMU 融合定位（基于 ROS + EKF）

1. 使用包依赖

2. 数据准备

输入数据：

可选增强数据：

3. EKF 节点配置文件（ekf.yaml）

4. 启动文件（ekf_fusion.launch）

5. 可视化与验证

推荐指令：

6. 问题排查建议

7. 总结