5.4 传感器数据接入与标准接口

5.4 传感器数据接入与标准接口

本节摘要：本章收束在数据入口：传感器的物理信号如何变成话题上的标准消息。本节梳理四大标准消息的类型与适用域，给出传感器 QoS 的正确配置，讲清每帧数据必须自带的两个元数据——时间戳与 frame_id——为何是下游一切算法的命根。学完本节，你应当能把一台新雷达或新相机接进系统，并且知道每个字段下游会怎么用。

标准接口是生态的合同

传感器接入的本质是一份三方合同：驱动厂商按标准消息类型发数据，算法按同一类型收数据，谁也不认识谁。这份合同让"换个雷达品牌、算法零改动"成为可能——前提是双方都守约。本节的任务是把你培养成守约的一方：选对类型、配对 QoS、填对元数据。

一、四大标准消息与选型

机器人最常用的传感器数据有四种标准形态，选型时对号入座：

两个字段的地位特殊，值得逐个交代。header.stamp：数据产生的物理时刻，不是到达时刻——驱动应当在读硬件时打点。SLAM 用它做扫描对齐，TF 用它查历史变换，2.2 节的插值查询吃的正是这个字段。header.frame_id：数据所在的坐标系声明，5.2 节那棵 TF 树的入口。下游拿着 frame_id 去查变换，查不到或者错位，5.3 节的漂移就来了。

# 雷达驱动的发布端骨架：两处元数据是合同义务
def hardware_callback(raw_scan):
    msg = LaserScan()
    msg.header.stamp = node.get_clock().now().to_msg()   # 产生时刻
    msg.header.frame_id = 'laser'                        # 与 TF 声明一致
    msg.angle_min = -3.14159
    msg.angle_max = 3.14159
    msg.range_min, msg.range_max = 0.1, 25.0
    msg.ranges = raw_scan.ranges                         # 米为单位的距离数组
    pub.publish(msg)
​

非标准数据也有去处：自定义消息类型（rosidl 定义，第 2.1 节的接口描述）或通用容器类型。原则是先找标准、再议自定义——自定义消息是生态合同上的一个洞，每个消费方都要为你单独适配。

二、传感器 QoS：合同里的履行条款

2.2 节讲过 QoS 合约，本节落到传感器场景的标准答案：传感器流用 sensor_data 预设（best_effort 加浅深度），控制与配置用 reliable。理由不再重复，这里补三条实操细则。

细则一：订阅端与驱动端必须同查。接不上一帧数据时，2.3 节的 info -v 两端对照是标准动作，别背 QoS 规则，直接看两端声明。细则二：深度按"处理周期乘以频率"估——处理一帧要 100 毫秒、数据 10 Hz，深度 2 到 3 就够，设 50 只是让过时数据排队。细则三：Imu 类高流小消息可以例外用 reliable——单帧太小，重传代价可忽略，完整性反而优先。

from rclpy.qos import qos_profile_sensor_data, QoSProfile

# 标准传感器订阅
sub = node.create_subscription(
    LaserScan, 'scan', on_scan, qos_profile_sensor_data)

# Imu 的例外配置：小消息用可靠传输
qos_imu = QoSProfile(reliability=1, depth=50)   # 1 即 RELIABLE
imu_sub = node.create_subscription(Imu, 'imu/data', on_imu, qos_imu)
​

图 5-3：一帧激光数据的入树旅程

三、现场要点：相机接入的两个专用话题

相机在四大类型之外多了两个工程话题，各值一段。编码转换：相机原生输出常是 YUV 或 Bayer 格式，算法要的是 BGR 或灰度，转换放在驱动端还是算法端是个真问题。我的取舍是驱动端只发布原生编码，转换放在消费端按需做——十个消费者五个要灰度、三个要彩色、两个要原始，驱动端替所有人做决定必然错一半。cv_bridge 只在消费端引入，驱动保持轻。

带宽预算：1080p 彩色帧约 6 MB，30 Hz 就是每秒 180 MB——跨机传输前先做这道算术。超预算的标准解法不是压缩 QoS（那改变的是可靠性语义），而是降分辨率、降帧率，或改发压缩消息类型（CompressedImage，JPEG 编码后一帧几十 KB）。RViz 看不到图像时，除了 2.3 节的 QoS 排查，也要想到带宽拥堵这一层。

⚠️ 常见坑：时间戳用"到达时刻"而非"产生时刻"，在跨机部署时会造成数十毫秒的系统级误差——所有插值与对齐一起跟着歪，且极难从症状反推。驱动代码审查时，stamp 的取值位置是必查项。

四、接入验收清单：新传感器上机五问

给系统接一台新传感器，验收不该凭感觉，而是过一遍固定清单。五问如下，每一问都有对应的工具与判据：

一问数据在流吗——topic hz 实测频率与标称值对账，差超过一成先查驱动配置再谈别的；二问元数据对吗——echo 单帧查时间戳是否在走、坐标系声明是否与 TF 一致，5.3 节的漂移事故正是这一问失守的代价；三问入树了吗——RViz 里点云与机器人模型的空间关系是否合理，悬空、翻转都是病灶显形；四问合约稳吗——info -v 两端对照，确认没有可靠对最优的静默不匹配；五问带宽扛得住吗——bw 实测乘以并发流数，对照网络与 CPU 预算做减法。

清单的出处是教训本身：本册前文每个传感器类事故——漂移、黑屏、互相失联——都能对应到五问中的某一问被跳过。把"老手的直觉"固化成"新手的路线"，是清单存在的全部理由。

另有一个性能侧的补充观察：接入后的系统负载增量主要取决于回调处理时长而非数据量。同样是每秒 10 MB 的数据流，回调里只做透传与做一次直方图统计，CPU 占用差出一个量级。所以验收之外，建议接入初期顺手记录回调耗时基线——日后性能退化时，这个基线是最有价值的对照物；没有基线，"变慢了"永远只是感觉。

本节要点回顾

• 四大标准消息各管一类数据，选型先标准后自定义，自定义类型是生态合同上的洞；
• stamp 与 frame_id 是合同义务：产生时刻打点、坐标系如实声明，下游一切对齐靠它们；
• 传感器 QoS 的标准答案是 sensor_data 预设，Imu 类小消息可例外用 reliable；
• 相机带宽先做算术：超预算降分辨率或改压缩消息，别动 QoS 语义；
• 编码转换放消费端按需做，驱动端保持轻与中立。

数据入口接好了，空间共识立住了。下一章把这些积木拼成真正的机器人能力：建图、导航与机械臂规划。