📚 课程目录第4章实时数据订阅与解析

4. 实时数据订阅与解析

RTDE 的实时数据订阅能力，核心在于 output recipe。它决定了控制器以固定周期向客户端推送哪些数据、以什么顺序排列、每个字段占多少字节。理解 output recipe，本质上就是理解“控制器如何把内部状态打包成一条二进制数据流”。如果 recipe 配置错误，后续的解析、时间戳对齐和控制决策都会失去意义。

一、output recipe 的配置逻辑

在 RTDE 中，output recipe 通过 RTDE_OUTPUT_SETUP 请求下发。客户端需要提供一份字段列表，控制器按列表顺序将对应数据写入每个周期的数据包。配置时需要注意三点：

字段顺序即字节顺序：recipe 中先声明的字段，在数据包中先出现。解析时必须严格按同一顺序读取，不能凭字段名猜测偏移。
字段类型决定字节长度：例如 DOUBLE 占 8 字节，UINT32 占 4 字节，VECTOR3D 占 24 字节，VECTOR6D 占 48 字节。总包长是各字段长度之和，不含额外头部。
recipe 一旦生效，周期内固定：运行中修改 recipe 需要重新协商，通常建议在启动阶段一次性确定，避免运行中切换导致解析错位。

一个典型的 output recipe 配置示例如下（字段名来自 RTDE 标准接口）：

output_recipe = [
  "timestamp",
  "actual_q",
  "actual_qd",
  "actual_TCP_pose",
  "actual_TCP_speed",
  "actual_TCP_force",
  "actual_digital_input_bits",
  "actual_digital_output_bits",
  "robot_mode",
  "safety_status"
]

这份 recipe 覆盖了时间戳、关节位置/速度、TCP 位姿/速度/力、数字 IO 和机器人状态。控制器会按此顺序逐字段打包，客户端按同样顺序逐字段解包。

二、关键数据字段的含义与量纲

不同字段的物理含义和单位必须明确，否则解析出的数值无法用于控制。以下按类别说明：

字段	类型	含义与单位
`timestamp`	DOUBLE	控制器时间戳，单位秒，通常为控制器启动后的单调时间
`actual_q`	VECTOR6D	六个关节的实际位置，单位 rad
`actual_qd`	VECTOR6D	六个关节的实际速度，单位 rad/s
`actual_TCP_pose`	VECTOR6D	TCP 在基坐标系下的位姿 [x,y,z,rx,ry,rz]，位置单位 m，姿态单位 rad（轴角）
`actual_TCP_speed`	VECTOR6D	TCP 速度 [vx,vy,vz,ωx,ωy,ωz]，线速度 m/s，角速度 rad/s
`actual_TCP_force`	VECTOR6D	TCP 处力/力矩 [Fx,Fy,Fz,Mx,My,Mz]，力单位 N，力矩单位 N·m
`actual_digital_input_bits`	UINT64	数字输入位图，每一位对应一个输入通道
`actual_digital_output_bits`	UINT64	数字输出位图，每一位对应一个输出通道
`robot_mode`	UINT32	机器人模式枚举，如运行、暂停、保护停止等
`safety_status`	UINT32	安全状态枚举，用于判断是否允许运动

需要特别注意的是 actual_TCP_pose 的姿态表示。RTDE 默认使用轴角（rotation vector）而非欧拉角，旋转向量的方向表示旋转轴，模长表示旋转角度。若上层算法使用旋转矩阵或四元数，需要在解析后做一次转换，不能直接把 rx、ry、rz 当作欧拉角使用。

三、数据包解析：从字节流到物理量

RTDE 数据包是纯二进制、无分隔符的连续字节流。解析过程就是按 recipe 顺序依次读取固定长度字段。以 Python 为例，常用 struct.unpack 按格式字符串解包：

import struct

# 假设 recipe 顺序为 timestamp, actual_q, actual_TCP_pose
fmt = "<d6d6d"   # 小端：1个double + 6个double + 6个double
packet_size = struct.calcsize(fmt)  # 8 + 48 + 48 = 104 字节

def parse_packet(data):
    timestamp, *rest = struct.unpack(fmt, data[:packet_size])
    actual_q = rest[0:6]
    actual_TCP_pose = rest[6:12]
    return timestamp, actual_q, actual_TCP_pose

解析时最容易出错的地方是字节序和偏移量。UR 控制器使用小端字节序，格式字符串必须以 < 开头。如果 recipe 中混有 UINT32、UINT64 和 DOUBLE，格式字符串要相应写成 <d6d6d6dQQII 这类组合，并确保总长度与控制器实际发送的包长一致。建议在连接建立后先读取一次包长并校验，避免因 recipe 不一致导致后续全部错位。

四、时间戳对齐与数据同步

RTDE 的 timestamp 来自控制器内部时钟，而客户端本地也有自己的接收时间。两者不能直接混用。常见做法是：

以控制器 timestamp 作为数据的时间基准，保证同一包内所有字段属于同一控制周期。
在客户端记录接收时刻 t_recv，用 t_recv - timestamp 估计传输延迟，用于监控链路健康度。
若需要与外部传感器融合，先做时钟偏移估计（如取多包延迟的中位数），再把外部数据对齐到控制器时间轴。

下面这张图展示了 output recipe 从配置到解析、再到时间戳对齐的完整数据链路，重点标出了字段顺序、字节长度和同步关系。

五、实践中的校验与排错

配置完成后，建议按以下顺序做一次最小验证：先只订阅 timestamp 和 actual_q，确认包长等于 8 + 48 = 56 字节；再逐步加入 actual_TCP_pose、actual_TCP_force 和数字 IO 字段，每加一个字段就重新核对包长。若解析出的关节角出现数量级异常（例如 1e-300 或 1e300），通常说明偏移量错位，而不是控制器数据本身有问题。

对于数字 IO，actual_digital_input_bits 和 actual_digital_output_bits 是位图。读取某一位时使用 (bits >> n) & 1，其中 n 是通道号。注意位图字段在 recipe 中的位置会影响后续字段的偏移，不能随意调整顺序。

建议：把 recipe 字段列表、格式字符串和包长写成一份配置常量，解析函数只依赖这份常量。这样当 recipe 变更时，只需改一处，避免解析代码和配置脱节。

时间戳对齐方面，如果只是做本地监控，直接用控制器 timestamp 作为横轴即可；如果要和视觉、力传感器等外部数据融合，建议先采集一段静止数据，计算 t_recv − timestamp 的均值和抖动，再决定是否做时钟偏移补偿。对于大多数实时控制场景，控制器时间戳的单调性和周期稳定性已经足够，关键是不要在客户端重新打时间戳覆盖它。

最后，output recipe 的字段越多，单包越大，网络和处理开销也越高。实际工程中应按需订阅：控制循环只需要关节位置、速度和 TCP 位姿；力控任务再增加 actual_TCP_force；IO 监控按需加入位图字段。保持 recipe 精简，是保证实时性的重要前提。