第 7 章 传感器驱动接入¶
传感器节点是 ROS 2 系统中数据的源头——相机发布 Image、激光雷达发布 LaserScan、IMU 发布 Imu 消息。驱动层的质量（时间戳精度、频率稳定性、话题命名规范）直接决定下游算法（SLAM、融合、控制）能否正常工作。本章按传感器类型逐一给出接入路径，并总结通用的驱动接入流程与时间同步机制。
1. 相机驱动¶
1.1 USB 摄像头（usb_cam）¶
usb_cam 是 V4L2 接口的通用 USB 摄像头驱动（Humble 包 ros-humble-usb-cam），可执行名 usb_cam_node_exe，节点 /usb_cam 默认设备 /dev/video0。最小启动：
sudo apt install -y 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 frame_id:=camera_link
输出话题：/image_raw（sensor_msgs/msg/Image）、/camera_info（sensor_msgs/msg/CameraInfo，含内参矩阵）。
1.2 深度相机（realsense2_camera）¶
Intel RealSense 系列通过 realsense2_camera 包接入（launch 入口 rs_launch.py，节点可执行名 realsense2_camera_node）。该驱动同时发布彩色图、深度图与对齐后的合成点云：
ros2 launch realsense2_camera rs_launch.py \
enable_color:=true enable_depth:=true \
align_depth.enable:=true \
base_frame_id:=camera_link # 相机基座 link；各传感器帧在其下由 tf_prefix 派生
典型输出话题：
| 话题 | 消息类型 | 内容 |
|---|---|---|
/color/image_raw |
sensor_msgs/Image |
RGB 彩色图 |
/depth/image_raw |
sensor_msgs/Image (32FC1) |
深度图（米） |
/depth/color/points |
sensor_msgs/PointCloud2 |
深度图按彩色内参投影到三维点云（align_depth.enable:=true） |
/color/camera_info |
sensor_msgs/CameraInfo |
彩色相机内参 |
/depth/camera_info |
sensor_msgs/CameraInfo |
深度相机内参 |
1.3 深度图 → 点云与 RGB-D 对齐¶
深度图转点云的数学依据是相机内参的逆变换：
depth_image_proc 包提供节点直接订阅深度图 + CameraInfo 发布 PointCloud2（point_cloud_xyz_node 只出几何；带彩色用 point_cloud_xyzrgb_node）：
ros2 run depth_image_proc point_cloud_xyz_node \
--ros-args -r image_rect:=/depth/image_raw -r camera_info:=/depth/camera_info
RGB-D 对齐：将深度像素对齐到彩色相机坐标系，使每个深度值可直接索引彩色像素。RealSense 驱动通过 align_depth.enable:=true 启用对齐滤镜（参数名取自 rs_launch.py）；外部传感器可用 tf2 + 外参标定做软件对齐。
2. 激光雷达驱动¶
2.1 rplidar（思岚系列）¶
rplidar_ros（Humble 包 ros-humble-rplidar-ros）支持 A1、S1、S2 等型号：
ros2 launch rplidar_ros rplidar_a1_launch.py \
serial_port:=/dev/ttyUSB0 frame_id:=laser_link
输出：/scan（sensor_msgs/msg/LaserScan）——一帧包含角度范围、角度分辨率与对应距离数组。
2.2 LaserScan 消息结构¶
sensor_msgs/msg/LaserScan:
std_msgs/Header header # 时间戳 + frame_id
float32 angle_min # 起始角 (rad)
float32 angle_max # 终止角 (rad)
float32 angle_increment # 角度分辨率 (rad/step)
float32 range_min # 最小有效距离 (m)
float32 range_max # 最大有效距离 (m)
float32[] ranges # 距离数组（每个角度一个值）
float32[] intensities # 反射强度（部分雷达支持）
RViz2 中 LaserScan Display 自动将 ranges[] 按极坐标展开为扇形点云。若显示为一团或形状异常，检查 angle_min/angle_max/angle_increment 与雷达实际扫描参数是否一致。
2.3 三维激光雷达与 PointCloud2¶
多线雷达（如 Livox、Velodyne/Ouster）输出 PointCloud2——按 XYZI 格式存储三维点。驱动通常自带 lidar_driver 包直接发布 PointCloud2；点云坐标系由 header.frame_id 指定。
3. IMU 驱动¶
3.1 Imu 消息结构¶
sensor_msgs/msg/Imu:
std_msgs/Header header
geometry_msgs/Quaternion orientation # 姿态（可选，无融合时为零）
float64[9] orientation_covariance # 对角 = 置信度，-1 = 不可用
geometry_msgs/Vector3 angular_velocity # 角速度 (rad/s)
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration # 线性加速度 (m/s²)
float64[9] linear_acceleration_covariance
IMU 驱动的 orientation 字段可选填——若输出纯原始数据（无姿态解算），orientation 全零且 orientation_covariance[0] = -1 表示不可用。下游姿态融合（Madgwick/EKF）从 angular_velocity 和 linear_acceleration 自行估计。
Imu 的类型组成（嵌套消息与定长协方差数组）：
classDiagram
class Imu {
+Header header
+Quaternion orientation
+Array9 orientation_covariance
+Vector3 angular_velocity
+Vector3 linear_acceleration
}
class Header {
+time stamp
+string frame_id
}
class Quaternion
class Vector3
Imu *-- Header
Imu *-- Quaternion
Imu *-- Vector33.2 数据频率与话题观测¶
IMU 典型输出频率 100~1000 Hz。高频数据对接入端的处理能力有要求：
## 检查实际发布频率
ros2 topic hz /imu/data
## 检查带宽（单帧 Imu 约 300 字节，200 Hz ≈ 60 KB/s）
ros2 topic bw /imu/data
topic hz / topic bw 对任一路传感器话题通用，是驱动接入后确认"在不在发、发多快"的第一道检查。
4. 传感器数据的可视化对照¶
数据形状对不对是检查的另一半，靠可视化工具看。按观察对象选入口：
| 数据 | RViz2 | Foxglove / PlotJuggler |
|---|---|---|
| 图像 | Image Display（占用主视口外的独立窗口） | Image Panel，支持 raw/compressed |
| 点云 | PointCloud2 Display | 3D Panel |
| 激光 | LaserScan Display | 3D Panel |
| IMU 曲线 | 不适合（无时序面板） | PlotJuggler 多通道曲线（第 4 章） |
| 姿态（orientation） | TF/Marker 间接观察 | 3D Panel 叠加 frame |
接入新驱动时的最小检查组合：topic hz 定频率 → RViz/Foxglove 看几何形状 → PlotJuggler 看数值随时间的漂移（如 IMU 零偏）。
5. GNSS 接收机接入¶
GNSS（Global Navigation Satellite System，全球导航卫星系统）接收机与相机/雷达的接入形态不同：它是双向低带宽文本流，而不是单向高带宽二进制流。
5.1 两条数据流：NMEA 输出与 RTCM 输入¶
flowchart LR
base["基准站 / NTRIP caster"] -->|"电台或 TCP：RTCM 3.x（改正层）"| rx["GNSS 接收机"]
rx -->|"UART：NMEA 0183 GGA/RMC/GST（状态层）"| drv["驱动节点"]
drv --> fix["/fix：NavSatFix"]
drv -.-> tr["/time_reference：TimeReference"]- NMEA 0183（美国国家海洋电子协会制定的 ASCII 语句标准）是状态层：GGA 带定位质量码、卫星数与 HDOP（见第 10 章「GNSS/RTK 观测与质量分档」），RMC 带速度与航向，GST 给出标准差。人可读、行数低，默认输出频率通常 1–10 Hz。
- RTCM（海事无线电技术委员会建议标准 10403.x，二进制）是改正层：需要接收机输入端拿到改正数才能进 RTK 解。链路两类——本地基准站的数传电台，或 NTRIP（Networked Transport of RTCM via Internet Protocol）客户端订阅 caster 的挂载点；后者要求上位机能出网与凭据配置（网络与代理侧见 linux-note 第 6、10 章）。
- 厂商二进制协议（如 u-blox 的 UBX）携带 NMEA 没有的信息（卫星状态、模糊度指标、配置写入）；驱动层选哪条不改变下游消息类型。
5.2 驱动节点与接入要点¶
生态侧常见包名目：nmea_navsat_driver（apt 名 ros-humble-nmea-navsat-driver，伴 nmea_msgs）与 ublox（apt 名 ros-humble-ublox-gps）；各接收机厂商另有自带 ROS 2 驱动的包。参数集与发布话题以各自项目仓库为准，不凭二手资料写推荐。
驱动输出的标准话题：/fix（sensor_msgs/msg/NavSatFix）为主，部分驱动另发 /time_reference（sensor_msgs/msg/TimeReference，承载卫星时间）与 /vel（geometry_msgs/msg/TwistStamped）。
## 串口直连：先看设备名与原始语句（不启驱动也能判模块是否在工作）
cat /dev/ttyACM0 | grep 'GNGGA' # 或 ttyUSB0；CH340/CP210x 类转接芯片见 linux-note 第 9 章
stty -F /dev/ttyUSB0 115200 cs8 -cstopb -parenb # 改波特率后接收机侧需一致，否则全是乱码
接入清单里 GNSS 特有的四条（通用六步仍适用）：
frame_id指向天线而不是base_link：NavSatFix的header.frame_id语义是天线位置（NavSatFix定义注释），天线到车体的偏移（lever arm，杆臂）必须在 URDF 里以静态 TF 给出（第 5 章）——否则绝对位置整体带一个固定偏置，且转弯时表现为系统性螺旋轨迹。- 质控字段不能省：驱动应按 GGA 质量码分档填
position_covariance与position_covariance_type（仅有 HDOP 时为APPROXIMATED）；
全填零的协方差会让第 10 章的滤波器把单点解当固定解信任。
3. 时间戳与授时分两件事：header.stamp 是主机 ROS 时间，卫星系统时间走 TimeReference；用 GNSS 做上位机授时时属系统侧议题（chrony / PTP 判据见 linux-note 第 6 章）。
4. QoS 与频率：1–10 Hz 的 /fix 用 sensor_data 预设即可；关键是不要丢包重传当优先级——陈旧改正数比缺一帧观测更坑（RTK 的模糊度固定依赖连续相位跟踪）。
与 ros2_control 的分界同样适用：GNSS 是纯数据输出型传感器，独立驱动节点即可，不进 controller_manager。
6. 驱动接入通用流程¶
接入任一新传感器驱动时的标准检查清单：
-
确认设备权限：串口设备（
/dev/ttyUSB*）需要dialout组权限或 udev 规则。sudo usermod -aG dialout $USER # 或写 udev 规则（/etc/udev/rules.d/99-usb-serial.rules）： SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE="0666" -
launch 参数核对：
serial_port、frame_id、baud_rate、波特率与硬件一致。 - TF 一致性：驱动输出的
header.frame_id必须与 URDF 中对应 link 名一致（第 5 章）。 - 时间戳来源：确认驱动打的是采集时刻（非发送时刻、非系统启动时刻）。
- QoS 设置：传感器数据默认使用
sensor_dataQoS profile（Best Effort + Keep Last 5）；下游订阅端须兼容。 - 频率与带宽检查：
ros2 topic hz和ros2 topic bw确认在预期范围。
7. image_transport 与压缩通路¶
直接发布原始 Image 消息的带宽极高（640×480 RGB 30fps ≈ 27.6 MB/s）。image_transport 提供插件化的传输编码：
sudo apt install -y ros-humble-image-transport-plugins
##include <image_transport/image_transport.hpp>
auto it = image_transport::ImageTransport(node);
auto pub = it.advertise("camera/image", 1);
// 发布原始 Image；订阅端加 /compressed 后缀即选择压缩传输
image_transport 的传输插件与发布/订阅接口由 C++ 提供，Humble 没有 Python 绑定。Python 节点直接发布 sensor_msgs/msg/Image，或把发布端写成 C++ 组件后由 Python 侧订阅。
| 编码 | 压缩率 | CPU 开销 | 适用 |
|---|---|---|---|
| raw | 无 | 无（直接 memcpy） | 机载处理、低延迟 |
| compressed (JPEG/PNG) | 中 | 编码耗时 | 跨机传输、bag 录制 |
| theora | 高 | 编码重 | 带宽极度受限 |
订阅端同样用 image_transport::Subscriber 透明选择编码格式——话题名加 /compressed 后缀即订阅压缩版本。
原始与压缩两路话题并存，订阅端按编码透明选择：
flowchart LR
drv["相机驱动"] --> raw["/image_raw<br/>640×480 RGB 30fps ≈ 27.6 MB/s"]
drv --> comp["/image_raw/compressed"]
raw --> sub["image_transport 订阅端<br/>按 .compressed 后缀选编码"]
comp --> sub
raw --> py["Python 节点<br/>直接订阅原始话题"]坑：image_transport 没有 Python 绑定
传输插件与发布/订阅接口都在 C++ 侧；Python 节点要么直接发原始 Image，要么订阅 C++ 组件发布的话题。
8. 多传感器时间同步¶
需要同时使用多个传感器数据时（如相机 + 点云融合、IMU + 里程计），须将不同话题的消息按时间戳配对。message_filters 提供两种策略：
近似同步（ApproximateTimeSynchronizer）——各话题的消息时间戳在窗口内取最近匹配：
from message_filters import Subscriber, ApproximateTimeSynchronizer
sub_cam = Subscriber(node, Image, '/camera/image_raw')
sub_lidar = Subscriber(node, PointCloud2, '/points2')
sync = ApproximateTimeSynchronizer([sub_cam, sub_lidar], queue_size=10, slop=0.1)
## slop=0.1 → 允许最大 100ms 时间差
def cb(img_msg, pc_msg):
# 两帧时间戳最接近时同时触发
...
sync.registerCallback(cb)
近似同步（ApproximateTime） vs 精确同步（TimeSynchronizer）：后者要求时间戳完全相同（实际中几乎不可能满足），前者在时间窗口内取最近匹配——绝大多数场景应使用 ApproximateTimeSynchronizer。
use_sim_time 下所有节点的时间戳来自 /clock，跨节点对齐精度取决于仿真器发布 /clock 的频率。真机环境下多传感器对齐依赖 NTP/PTP 同步 + 硬件触发（如相机外触发与雷达同步脉冲）。
判据：多传感器对齐用近似同步
相机与雷达频率不同，先按时间窗做近似同步（ApproximateTime），窗口宽度由控制周期反推；追求严格同时刻只会一直等不到配对。
9. 自写传感器节点最小路径¶
对于无现成 ROS 驱动的传感器（自制板卡、串口设备），最小路径是写一个独立节点直接发布标准消息：
"""自写 IMU 节点——从串口读数据，发布 sensor_msgs/Imu"""
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
import serial, struct
class ImuDriver(Node):
def __init__(self):
super().__init__('imu_driver')
self.pub = self.create_publisher(Imu, '/imu/data', 10)
self.serial = serial.Serial('/dev/ttyUSB1', 115200, timeout=0.01)
self.timer = self.create_timer(0.01, self.read_and_publish) # 100 Hz
def read_and_publish(self):
raw = self.serial.read(24) # 6 个 float32 (3 gyro + 3 accel)
if len(raw) < 24:
return
gx, gy, gz, ax, ay, az = struct.unpack('<6f', raw)
msg = Imu()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = 'imu_link' # 须与 URDF 一致
msg.angular_velocity.x = gx # rad/s
msg.linear_acceleration.x = ax # m/s²
# ... 其余字段 ...
self.pub.publish(msg)
rclpy.init()
rclpy.spin(ImuDriver())
该路径与 ros2_control 的 hardware_interface 分工不同：ros2_control 适合执行器（电机） 的闭环控制；纯数据输出型传感器（IMU、GNSS 接收机（见「GNSS 接收机接入」）、超声）用独立节点更简洁，无需引入 controller_manager 开销。
10. 要点¶
- 相机驱动的要点是 CameraInfo 发布（供下游标定与投影）和 image_transport 选择（raw vs compressed）；RGB-D 对齐决定点云与彩色图的像素级对应关系。
- LaserScan 的几何参数（angle_min/max/increment、range_min/max）必须与雷达实际扫描一致；RViz 中显示异常首查这些参数。
- IMU 消息的 covariance 字段决定下游 EKF（robot_localization）的信任权重；
orientation_covariance[0] = -1表示姿态不可用。 - GNSS 接入是两条流：NMEA 0183 是状态层（质量码、DOP），RTCM 3.x 是改正层（电台或 NTRIP 入口）；
frame_id指向天线、协方差按质量档分档填，是 GNSS 特有且最容易做错的两条。 - 驱动接入六步清单（权限→参数→TF→时间戳→QoS→频率检查）覆盖 90% 的"驱动装好了但数据不对"问题。
- 接入检查分两道：
topic hz/bw确认频率与带宽，RViz2/Foxglove/PlotJuggler 按数据类型确认形状与漂移——图像与点云看几何，IMU 看时序曲线。 message_filters.ApproximateTimeSynchronizer是多传感器融合的入口；slop参数决定可容忍的最大时间差。- 纯数据型传感器用独立节点最轻量；ros2_control 适合需要闭环命令的执行器场景。