📚 课程目录第3章传感器组件

3、传感器组件：激光雷达、摄像头、IMU等传感器的ROS2驱动与数据读取

3.1 传感器在ROS2中的角色与驱动模型

机器人感知系统的核心任务是将物理世界的连续信号转换为机器人可处理的离散数据。在ROS2体系下，每种传感器都通过独立的驱动节点完成数据采集与协议解析，再以标准消息类型发布到对应话题。驱动节点通常运行在机器人主控或专用传感器计算单元上，其内部结构可抽象为三层：硬件接口层（串口、以太网、USB或CAN）、协议解析层（将厂商私有数据帧转换为ROS2标准消息）、生命周期管理层（负责参数配置、错误恢复与状态上报）。

理解这一模型对后续开发至关重要。以激光雷达为例，不同厂商（如思岚、速腾、镭神）的底层协议差异极大，但通过驱动节点转换后，上层应用只需订阅统一的 sensor_msgs/msg/LaserScan 或 sensor_msgs/msg/PointCloud2 话题即可，无需关心具体硬件差异。这种“驱动隔离”设计正是ROS2传感器生态的核心价值。

3.2 激光雷达驱动与数据读取

激光雷达是移动机器人建图与导航的核心传感器。在ROS2中，2D激光雷达通常发布 sensor_msgs/msg/LaserScan 消息，包含角度范围、角度分辨率、距离数组等字段；3D激光雷达则发布 sensor_msgs/msg/PointCloud2，以点云形式表达三维空间信息。以思岚A1/A2系列为例，其ROS2驱动包 sllidar_ros2 的启动方式如下：

# 安装驱动（以Humble为例）
sudo apt install ros-humble-sllidar-ros2

# 启动雷达驱动节点
ros2 launch sllidar_ros2 sllidar_a1_launch.py

# 查看话题列表
ros2 topic list | grep scan

# 可视化激光数据（RViz2中订阅 /scan）
ros2 run rviz2 rviz2

驱动启动后，可通过命令行直接查看数据内容：

# 打印前10帧激光数据
ros2 topic echo /scan --once | head -20

# 输出结果示例：
# header:
#   stamp:
#     sec: 1710000000
#     nanosec: 123456789
#   frame_id: "laser"
# angle_min: 0.0
# angle_max: 6.2831902503967285
# angle_increment: 0.01745329238474369
# range_min: 0.12
# range_max: 12.0
# ranges: [0.45, 0.46, 0.44, ...]

在编写订阅节点时，需要注意 frame_id 必须与TF树中的坐标系名称一致，否则后续建图或导航时会出现坐标变换错误。典型的数据读取代码片段如下：

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan

class LaserSubscriber(Node):
    def __init__(self):
        super().__init__('laser_subscriber')
        self.sub = self.create_subscription(
            LaserScan, '/scan', self.scan_callback, 10)
    
    def scan_callback(self, msg):
        # 获取正前方距离（角度0附近）
        mid_idx = len(msg.ranges) // 2
        front_dist = msg.ranges[mid_idx]
        self.get_logger().info(f'前方障碍距离: {front_dist:.2f} m')

rclpy.init()
node = LaserSubscriber()
rclpy.spin(node)

3.3 摄像头驱动与图像数据获取

视觉传感器在ROS2中通常使用 usb_cam 或 v4l2_camera 驱动。对于USB摄像头，usb_cam 是最常用的驱动包，支持参数动态配置。启动方式如下：

# 安装usb_cam驱动
sudo apt install ros-humble-usb-cam

# 启动摄像头节点（指定设备与分辨率）
ros2 run usb_cam usb_cam_node_exe --ros-args \
  -p video_device:=/dev/video0 \
  -p image_width:=640 \
  -p image_height:=480 \
  -p pixel_format:=yuyv \
  -p camera_frame_id:=camera_link

# 查看图像话题
ros2 topic list | grep image

# 使用rqt_image_view可视化
ros2 run rqt_image_view rqt_image_view

摄像头驱动发布的话题通常包括：

/image_raw：原始图像（sensor_msgs/msg/Image）
/camera_info：相机内参及畸变系数（sensor_msgs/msg/CameraInfo）
/image_raw/compressed：JPEG压缩图像（用于带宽受限场景）

在图像处理节点中，推荐使用 cv_bridge 将ROS2图像消息转换为OpenCV格式，便于后续算法处理：

import cv2
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

class ImageProcessor(Node):
    def __init__(self):
        super().__init__('image_processor')
        self.bridge = CvBridge()
        self.sub = self.create_subscription(
            Image, '/image_raw', self.image_callback, 10)
    
    def image_callback(self, msg):
        try:
            cv_img = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
            # 在此处添加图像处理算法
            gray = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
            cv2.imshow('Gray View', gray)
            cv2.waitKey(1)
        except Exception as e:
            self.get_logger().error(f'图像转换失败: {e}')

时间戳同步提示：当需要融合激光雷达与摄像头数据时，务必检查两者消息头中的 stamp 时间戳。若传感器硬件不支持时间同步，可在驱动参数中启用软件时间同步（如 approx_sync 策略），但会引入一定延迟。对于高动态场景，建议使用支持PTP（IEEE 1588）的硬件。

3.4 IMU驱动与姿态数据读取

IMU（惯性测量单元）提供三轴加速度、三轴角速度以及（部分型号）三轴磁场强度数据。在ROS2中，标准消息类型为 sensor_msgs/msg/Imu，包含线加速度、角速度及对应的协方差矩阵。以BMI088或MPU6050为例，常用驱动为 imu_filter_madgwick 配合底层I²C/SPI驱动。

IMU驱动启动示例（以 bmi088_driver 为例）：

# 启动IMU驱动节点
ros2 run bmi088_driver bmi088_node --ros-args \
  -p port:=/dev/i2c-1 \
  -p address:=0x68 \
  -p rate:=200 \
  -p frame_id:=imu_link

# 查看IMU数据
ros2 topic echo /imu/data --once

# 输出示例：
# header:
#   stamp:
#     sec: 1710000000
#     nanosec: 987654321
#   frame_id: "imu_link"
# orientation:
#   x: 0.0012
#   y: -0.0034
#   z: 0.9999
#   w: 0.0123
# angular_velocity:
#   x: 0.01
#   y: -0.02
#   z: 0.05
# linear_acceleration:
#   x: 0.12
#   y: -0.08
#   z: 9.81

IMU数据使用前通常需要零偏校准和滤波融合。零偏校准可在静止状态下采集100~200帧数据求平均；滤波融合则常用Madgwick或互补滤波算法，将加速度计与陀螺仪数据融合为稳定的姿态四元数。ROS2中可使用 imu_filter_madgwick 节点完成该任务：

# 启动Madgwick滤波节点
ros2 run imu_filter_madgwick imu_filter_madgwick_node --ros-args \
  -p use_mag:=false \
  -p gain:=0.1 \
  -p publish_tf:=true

# 滤波后输出 /imu/data 话题，可直接用于里程计推算

3.5 多传感器时间同步与数据融合案例

在实际机器人系统中，单一传感器往往无法满足可靠感知需求。以轮式机器人导航为例，常采用激光雷达+IMU+轮式里程计的融合方案：激光雷达提供高精度环境距离信息，IMU提供高频姿态变化，轮式里程计提供位移增量。三者通过EKF（扩展卡尔曼滤波）融合，可获得更平滑、更准确的位姿估计。

ROS2中可使用 robot_localization 包实现多传感器融合，配置要点如下：

传感器	话题名称	消息类型	频率	融合用途
2D激光雷达	/scan	LaserScan	10 Hz	建图、定位、避障
IMU	/imu/data	Imu	200 Hz	姿态估计、航向角
轮式里程计	/odom	Odometry	50 Hz	位移增量、速度反馈
摄像头	/image_raw	Image	30 Hz	目标检测、语义分割

时间同步是融合的关键。推荐做法是：在驱动节点中启用硬件时间戳（若传感器支持），否则使用 message_filters 的 ApproximateTimeSynchronizer 进行近似时间对齐。以下代码展示了如何同步激光雷达与IMU数据：

from message_filters import ApproximateTimeSynchronizer, Subscriber
from sensor_msgs.msg import LaserScan, Imu

# 创建同步器（容忍时间差50ms）
scan_sub = Subscriber(node, LaserScan, '/scan')
imu_sub = Subscriber(node, Imu, '/imu/data')
sync = ApproximateTimeSynchronizer(
    [scan_sub, imu_sub], queue_size=10, slop=0.05)
sync.registerCallback(self.sync_callback)

def sync_callback(self, scan_msg, imu_msg):
    # 此处两个消息的时间戳已对齐
    # 可进行联合特征提取或状态预测
    pass

3.6 常见误区与排障复盘

误区一：忽略frame_id一致性。许多初学者在启动多个传感器后，发现RViz2中数据不显示或位置错乱，根本原因是各传感器的 frame_id 未正确设置，且TF树中缺少对应的坐标变换。务必在驱动参数中为每个传感器指定唯一的坐标系名称，并在启动文件中发布静态TF变换。

误区二：直接使用原始IMU数据。未经校准和滤波的IMU数据包含大量噪声和零偏，直接用于姿态估计会导致积分漂移。正确做法是先进行静止校准，再通过滤波算法融合，最后才用于控制或导航。

在排障过程中，建议按以下顺序检查：

硬件连接检查：使用 lsusb、dmesg 确认设备是否被系统识别；对于串口设备，检查 /dev/ttyUSB* 权限（sudo chmod 666 /dev/ttyUSB0）。
驱动状态检查：ros2 node list 确认驱动节点是否正常运行；ros2 topic hz /scan 查看发布频率是否符合预期。
数据内容检查：ros2 topic echo 查看消息内容是否合理（如距离值是否在有效范围内、时间戳是否更新）。
TF树检查：ros2 run tf2_tools view_frames 生成TF树PDF，确认所有坐标系正确连接。

最后，建议在开发初期就建立统一的传感器数据采集与回放流程。使用 ros2 bag record 录制传感器数据，便于离线调试算法：

# 录制所有传感器数据
ros2 bag record /scan /image_raw /imu/data /odom

# 回放数据（可重复调试）
ros2 bag play rosbag2_2024_03_15-10_30_00

通过规范化的驱动配置、时间同步和调试流程，可以大幅提升多传感器系统的稳定性和数据质量，为后续建图、导航与感知算法奠定可靠基础。