📚 课程目录第4章 ROS2中的传感器数据接入与处理

4. ROS2中的传感器数据接入与处理

海洋无人装备（USV、AUV、ROV）的自主能力，首先取决于它能否稳定、准确地“感知”自身状态与周围环境。在ROS2系统中，这一感知过程被统一抽象为传感器数据接入：底层硬件通过驱动节点把原始电信号或串口报文转换为标准化的 sensor_msgs 消息，再经由话题（Topic）发布到计算图中，供导航、控制、感知等上层节点订阅使用。本节围绕GPS、IMU、深度计、DVL四类海洋装备常用传感器，讲解驱动节点的编写思路、消息类型选择，以及用 rqt_plot 与 rosbag2 做可视化与录制的完整流程。

一、四类传感器的消息类型与语义

ROS2为常见传感器定义了标准消息，接入时应优先复用，而不是自定义消息，这样上层算法与可视化工具才能直接兼容。四类传感器的对应关系如下：

传感器	典型输出	推荐消息类型	关键字段
GPS	经纬度、海拔、定位质量	`sensor_msgs/msg/NavSatFix`	latitude、longitude、altitude、status、position_covariance
IMU	三轴角速度、三轴线加速度、姿态	`sensor_msgs/msg/Imu`	angular_velocity、linear_acceleration、orientation
深度计	压力换算的吃水深度	`sensor_msgs/msg/FluidPressure` 或自定义深度	fluid_pressure、variance
DVL	对底三轴速度、离底高度	`geometry_msgs/msg/TwistWithCovarianceStamped`	twist.twist.linear、covariance

需要特别强调坐标系约定：ROS2遵循右手系，x向前、y向左、z向上。IMU的角速度与线加速度、DVL的速度都必须在该机体坐标系（通常命名为 base_link 或 imu_link）下表达。深度计若输出“向下为正”的深度值，接入时应明确转换为 z 轴向上约定下的负值，或在消息的 frame_id 与文档中写清符号定义，避免后续控制节点把“下潜”误判为“上浮”。

二、驱动节点的编写结构

一个规范的传感器驱动节点通常包含四部分：参数声明、硬件/仿真数据源初始化、定时器回调中的“读取—转换—发布”、以及生命周期管理。下面以IMU为例给出核心骨架（示例参数为教学假设值，非实测）：

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
from builtin_interfaces.msg import Time

class ImuDriver(Node):
    def __init__(self):
        super().__init__('imu_driver')
        self.declare_parameter('port', '/dev/ttyUSB0')
        self.declare_parameter('rate_hz', 100.0)
        self.pub = self.create_publisher(Imu, 'imu/data_raw', 10)
        period = 1.0 / self.get_parameter('rate_hz').value
        self.timer = self.create_timer(period, self.on_timer)

    def on_timer(self):
        raw = self.read_sensor()          # 读取原始报文
        msg = Imu()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'imu_link'
        msg.angular_velocity.x = raw['gz']
        msg.linear_acceleration.x = raw['ax']
        # 其余轴同理，并填写协方差
        self.pub.publish(msg)

    def read_sensor(self):
        return {'gz': 0.0, 'ax': 0.0}     # 占位，实际解析串口/网络

几个工程要点值得注意。第一，header.stamp 应尽量使用传感器采样时刻，而非发布时刻，否则多传感器融合时会产生时间偏差；若硬件不支持时间戳，可在驱动内做固定延迟补偿。第二，frame_id 必须与URDF中的连杆名称一致，否则 robot_state_publisher 与TF查询会失败。第三，发布频率由 rate_hz 参数控制，GPS通常1～10 Hz，IMU可达100～400 Hz，DVL约1～10 Hz，深度计1～5 Hz，频率不匹配会导致下游滤波器丢帧或过载。第四，队列长度（QoS depth）对高频IMU建议设为10以上，对低频GPS设为5即可。

对于GPS，还需处理定位状态：NavSatFix.status.status 取 STATUS_FIX、STATUS_NO_FIX 等枚举值，当无定位时应发布 STATUS_NO_FIX 而非伪造坐标，让上层节点据此降级。DVL在“对底锁定”与“对水跟踪”两种模式下速度可信度差异很大，建议在自定义消息或协方差中体现模式标志。

三、数据可视化与录制

驱动节点跑起来后，第一步是确认数据“活着且合理”。用 ros2 topic hz /imu/data_raw 检查频率是否接近设定值，用 ros2 topic echo 抽查字段。随后用 rqt_plot 观察时序曲线：

ros2 run rqt_plot rqt_plot \
  /imu/data_raw/angular_velocity/z \
  /dvl/twist/twist/linear/x \
  /gps/fix/latitude

在 rqt_plot 中，静止时IMU角速度应围绕零附近小幅波动，若出现明显偏置（例如 gz 长期为0.02 rad/s），说明需要标定零偏；DVL在静止水面应接近0 m/s，若持续跳变则可能是底跟踪丢失。深度计曲线应平滑单调，突跳往往意味着压力传感器受气泡或温度影响。

录制数据用 rosbag2，建议按传感器分组录制，便于回放时选择性加载：

ros2 bag record -o dive_001 \
  /imu/data_raw /gps/fix /depth/pressure /dvl/twist

回放时用 ros2 bag play dive_001 --rate 0.5 慢速复现，配合 rqt_plot 重新观察，可验证算法在相同输入下的可重复性。录制前应确认磁盘空间与消息体积：100 Hz的IMU单条消息约几百字节，一小时约百MB量级，长时间任务需评估存储。

四、接入质量的判断依据

数据接入是否成功，不能只看“话题有输出”，而应从三个层面判断。其一是频率与时间戳：ros2 topic hz 显示的实际频率应在设定值±10%内，且 ros2 topic delay 或回放对比中时间戳单调递增、无大面积跳变。其二是数值合理性：静止状态下IMU角速度均值接近零、DVL速度接近零、GPS的 status 为有效定位、深度计读数与已知水深一致。其三是协方差与状态字段：position_covariance、linear_acceleration_covariance 等字段不能全为零，否则EKF等融合节点会因“过度自信”而发散；无定位时应显式置 STATUS_NO_FIX。

建议在驱动节点中加入一个轻量自检：每收到N帧统计一次频率与数值范围，超出阈值时用 RCLCPP_WARN 打印，便于在长航时任务中及早发现传感器掉线或漂移。

把上述流程串起来，就形成了海洋无人装备感知链路的最小闭环：传感器→驱动节点→标准 sensor_msgs 话题→可视化与录制→供融合与控制使用。后续的定位融合、路径跟踪等章节，都将建立在这一稳定、语义清晰的数据基础之上。