TeleAvatar 2.0 开发者文档

适用范围

本文档用于说明软件接入、接口调用和示例程序。产品级安全、开机、关机、急停、维修与 VR 遥操作须同时遵守 TeleAvatar 2 遥操作机器人系统用户手册 V0.1。如发现两份文档的技术信息不一致，请停止相关操作并联系技术支持确认。

01 产品、机型与安全

1.1 产品概览

灵御 TA2（TeleAvatar 2）是一款轮式双臂人形遥操作机器人，配备双 7 轴机械臂、定制化夹爪、双目视觉系统和三自由度全向轮底盘，并可选配升降立柱。本开发接口覆盖双臂、夹爪、底盘与升降机构的 ROS 2 控制和状态访问能力。

1.2 支持机型与开发能力矩阵

1.3 开发前安全检查

运行机械臂、夹爪、底盘或升降机构前：

1. 确认机器人周围没有人员、障碍物或易碰撞物体，并具备安全运动空间。

1. 检查电源线、网线、相机线缆和机械臂线缆连接正常，无明显松动、破损或异常弯折。

1. 初次 API 测试应先读取当前状态，将该状态原样回发以验证链路，再进行小幅目标变化；不要一次发送与当前状态差距很大的目标。

1. 发生异常运动时，立即停止发送控制指令或发送失能指令。物理急停、上电与关机操作遵循用户手册。

1.4 停止与恢复动作

02 快速开始

2.1 环境要求

2.2 获取机器人 IP 与网络连接

TA2 默认通过有线网络连接至局域网，用户主机也应通过有线方式连接至同一局域网。可通过 remoteApp 查看本体 IP，下文统一以 <ROBOT_IP> 表示。核心板 IP 同时用于访问机器人管理、数据录制、文件下载和 VR 遥操作页面。

使用HDMI线将显示器直接连接至设备的视频接口，即可进入此页面。如下图可查看<ROBOT_IP>

针对与二开版本的配置界面：

在底部导航栏（如上图所示）选择 系统配置，即可进入下图所示的配置面板：

界面各项配置说明：

操作说明：

• 保存配置：将界面上的修改写入配置文件（持久化，重启后仍生效）。

• 生效：使当前配置立即应用到运行中的系统。

• 关闭：退出配置面板。

2.3 配置 API 模式与部件

用户可通过 remoteApp 或显示器直连本体视频接口进入“系统配置”。修改后应先点击保存配置，再点击生效，之后物理重启机器人以确认电机正常被使能。

2.4 接入 ROS 2 Topic

在用户主机设置 ROS 环境并启动 bridge：

export ROS_DOMAIN_ID=29
export ROS_DISTRO=humble
zenoh-bridge-ros2dds -e tcp/<ROBOT_IP>:9000

另开终端，设置相同 ROS 域并检查话题：

export ROS_DOMAIN_ID=29
ros2 topic list
ros2 topic echo /right_arm/joint_states

• bridge 终端需保持运行，关闭即断开互联。

• 订阅终端必须与 bridge 使用相同 ROS_DOMAIN_ID=29。

• 若用户主机在容器中运行，容器网络须能访问 <ROBOT_IP>:9000。

2.5 首次安全验证：初始位置示例

首次接入建议使用“初始位置示例”验证控制链路，示例代码见 §6.6。将对应机械臂配置为关节控制模式后运行示例，机械臂将运动到示例定义的初始姿态。运行前应确认所需部件已启用，并使用与本体一致的 ROS 域。

2.6 开发主机环境补充配置

ROS 域、bridge 启动命令和图像接收依赖见 §2.1、§2.4 和 §3.2，本节仅补充安装来源及网络配置要求。

2.6.1 安装 zenoh bridge

通过 Eclipse Zenoh 官方 APT 仓库安装：

sudo apt update
sudo apt install -y curl gnupg
sudo install -d -m 0755 /etc/apt/keyrings

curl -fsSL \
  https://download.eclipse.org/zenoh/debian-repo/zenoh-public-key \
  | sudo gpg --dearmor --yes \
      --output /etc/apt/keyrings/zenoh-public-key.gpg

echo \
  "deb [signed-by=/etc/apt/keyrings/zenoh-public-key.gpg] https://download.eclipse.org/zenoh/debian-repo/ /" \
  | sudo tee /etc/apt/sources.list.d/zenoh.list >/dev/null

sudo apt update
sudo apt install -y zenoh-bridge-ros2dds

检查安装结果并记录实际版本：

zenoh-bridge-ros2dds --version

当前开发主机通过 §2.4 的命令行参数连接机器人，不需要额外的 bridge 配置文件。

2.6.2 RMW/DDS

无需配置特殊 RMW 实现，使用 ROS 2 Humble 默认的 Fast DDS。若其他 ROS 工作区设置过 RMW_IMPLEMENTATION，启动前清除：

unset RMW_IMPLEMENTATION
source /opt/ros/humble/setup.bash

2.6.3 网络补充要求

• 9000/tcp：开发主机连接机器人 zenoh 路由器。

• 8890/udp：机器人向开发主机推送 RTP/H.265 视频。

• 多网卡主机应使用以下命令确认访问机器人所用的出口网卡和源 IP：

ip route get <ROBOT_IP>

其中 src 地址应与系统配置中的“远端 IP”一致。若开发主机启用了 UFW，按机器人 IP 放行视频端口：

sudo ufw allow from <ROBOT_IP> to any port 8890 proto udp

当前版本使用系统默认 MTU，不要求用户主动修改。禁止将机器人控制端口暴露到公共网络。

2.6.4 支持边界与验收

• 当前图像接收仅验证 §3.2 所述的 NVIDIA 硬件解码路径，暂不提供软件解码回退方案。

• apt 和 pip 依赖按照本文现有安装清单执行，不另行提供精确依赖锁。

• 1. §2.4 的 ros2 topic list 和状态 Topic 读取；
• 1. §5.3 的视频首帧接收测试；
• 1. §2.5 的最小安全状态测试。

2.7 Bring-up 与验收

上手步骤见 §2.2–§2.6，本节仅列出验收项目和通过条件。当前版本尚未提供统一的 smoke test 脚本，需按下列项目逐项执行并记录结果。

2.7.1 通信与状态检查

保持 §2.4 的 bridge 运行，在另一终端执行：

nc -z -w 3 <ROBOT_IP> 9000
ros2 topic list
ros2 topic echo --once /right_arm/joint_states
ros2 topic echo --once /api/current_mode
ros2 topic hz /right_arm/joint_states

通过条件：

• 9000/tcp 连接成功；

• 已启用部件对应的 API 和状态 Topic 可见；

• 10 秒内收到有效状态消息，且状态持续更新；

• /api/current_mode 与 §2.3 中已保存并生效的运行模式一致。机器人侧状态 Topic 的标称频率见 §4.1.1。经 bridge 后的实际到达频率可能受网络影响，本项只记录实测值，不设置固定阈值。

2.7.2 视频检查

按 §5.3 运行 10 秒视频接收测试。通过条件：

• 10 秒内收到首帧；

• RTP 包数和图像帧数持续增加；

• 输出目录生成 6 个分割视频；

• 记录日志中的实测 FPS。

2.7.3 实机控制检查

以下检查会引起机器人运动，必须先完成 §1.3 的安全检查，并确保操作人员可以随时执行物理急停。

03 通信与架构

3.1 通信路径

控制与状态数据使用 ROS 2；本体侧启动 zenoh 路由器，用户主机运行 zenoh-bridge-ros2dds 接入该路由器。接入后，用户可在本机发现本体 ROS 2 Topic，并向 /api 控制 Topic 发布指令。图像不通过 ROS 2 Topic 传输，而是由本体拼接、编码后通过 RTP 推送至用户主机。

3.1.1 数据方向

• 控制与状态通过 9000/tcp 双向传输；用户侧使用 /api/* 控制 Topic 和公开状态 Topic。

• 视频通过 8890/udp 从本体单向推送至用户主机，不占用 ROS 2 Topic。

• API 模式只接收 API 指令，VR 模式只接收 VR 指令；运行模式按 §2.3 配置并生效。

• /api/fsm/enable 由 FSM 直接接收，不经过 MUX，用于 API 模式下机械臂与夹爪控制链路的使能和心跳。

3.1.2 视觉链路术语

“4 路”指编码前采集通道，“6 路”指客户端从拼接画面裁剪出的子画面，两者不是同一层级。单个腕部相机为一路拼接好了左右双目的采集通道。

3.2 图像接收依赖

接收端（Ubuntu）安装：

sudo apt update
sudo apt install -y \
  python3-gi gir1.2-gstreamer-1.0 gstreamer1.0-tools \
  gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-bad \
  python3-opencv python3-numpy

默认解码器是 nvh265dec max-display-delay=0。确认 NVIDIA 驱动正常后，执行：

nvidia-smi
for e in rtph265depay h265parse nvh265dec videoconvert appsink; do
  gst-inspect-1.0 "$e" >/dev/null && echo "$e OK" || echo "$e MISSING"
done

3.3 网络安全、控制权限与多客户端边界

当前通信链路按可信局域网设计。安全边界依赖控制网络隔离和防火墙；禁止将机器人控制端口暴露到公共网络。

3.3.1 端口与安全边界

ROS_DOMAIN_ID=29 仅用于区分 ROS 2 通信域，不是鉴权机制。能够接入该网络和 zenoh 路由器的主机可能发现 Topic 并向控制 Topic 发布消息。最小放行规则：

• 机器人侧仅允许授权开发主机访问 9000/tcp；

• 开发主机侧按 §2.6 仅允许机器人 IP 访问 8890/udp；

• 机器人和授权开发主机应位于独立控制网段，与办公网、访客网和公共网络隔离；

• 本体内部服务和未列入公开接口的端口不得向开发网络开放。

3.3.2 控制所有权

当前版本同一台机器人在同一时间只支持一个控制程序，不支持多个客户端同时发布控制指令。

• 多客户端可以只读订阅状态 Topic。

• 视频按系统配置中的单个“远端 IP”推送，多客户端同时接收未经验证。

• 多个程序不得同时向 /api/* 或 /api/fsm/enable 发布消息。

• 调试用的 ros2 topic pub 不得与控制程序同时向同一 Topic 发布。切换控制程序前，应先停止原程序及其心跳发布，再由新程序读取机器人当前状态、同步控制目标并重新使能。多人协作时必须通过现场流程明确当前控制方，并确保操作人员可以随时执行物理急停。

04 ROS 2 接口参考

4.0 坐标系、关节顺序与限位约定

本章公开接口的坐标系、关节顺序和软件限位以本节给出的配置为准。该配置随文档版本维护，用户无需访问机器人内部工程文件。

4.0.1 机械臂坐标系

左右机械臂分别以各自肩部为坐标原点，不共用同一个位置原点：

/api/{arm_side}_arm/target_pose 和 /{arm_side}_arm/current_ee_pose 均使用对应机械臂的肩部坐标系。左右臂位姿不能在不进行坐标变换的情况下直接交换。

4.0.2 末端位姿约定

末端目标和末端状态使用 geometry_msgs/msg/Pose：

• position 表示末端在对应肩部坐标系中的位置，单位为 m；

• orientation 表示末端相对对应肩部坐标系的姿态；

• 四元数字段顺序为 [x, y, z, w]；

• Pose 不包含 frame ID 和时间戳，需要时间对齐时由接收端记录接收时间；

• 发送目标前应先读取当前末端位姿，并从当前状态平滑起步。

4.0.3 关节名称与数组顺序

每条机械臂包含 7 个关节。关节控制和状态消息中的数组必须严格按照下表排列，关节位置单位为 rad：

关节目标数组必须为 7 维，且不得仅修改 name 字段而保持错误的 position 顺序。七个关节全部为零不代表初始姿态，也不应作为默认控制目标。

4.0.4 机械臂配置

以下配置定义当前版本公开接口使用的自由度、软件限位、末端位姿参考系和关节名前缀：

robot_name: TeleAvatar
arms:
  dof_num: 7
  max_arm_len: 0.55
  right_arm:
    upper: [1.8, 0.0, 2.6, -0.25, 1.8, 1.4, 0.7]
    lower: [-1.8, -1.9, -1.3, -2.2, -1.8, -1.4, -0.7]
  left_arm:
    upper: [1.8, 1.9, 1.3, 2.2, 1.8, 1.4, 0.7]
    lower: [-1.8, 0.0, -2.6, 0.25, -1.8, -1.4, -0.7]

ik_solver:
  right_arm:
    shoulder_frame_name: right_shoulder_base
    target_ee_frame_name: right_target_ee_frame
    current_ee_frame_name: right_current_ee_frame
    joint_name_prefix: r_joint
  left_arm:
    shoulder_frame_name: left_shoulder_base
    target_ee_frame_name: left_target_ee_frame
    current_ee_frame_name: left_current_ee_frame
    joint_name_prefix: l_joint

该配置是面向用户的接口摘要，不包含内部控制参数。关节限位单位为 rad，max_arm_len 单位为 m。

4.0.5 内部模型边界

机器人内部使用 URDF 完成运动学和碰撞计算，但该模型及其 link、joint 和 frame 命名不属于公开 ROS 2 接口。当前版本不对外定义法兰、TCP、用户 tool frame 或 IK 内部坐标变换。

4.1 接口总览

下表以用户侧开发节点为视角描述操作方向：“发布”表示用户节点向机器人发布控制消息；“订阅”表示用户节点订阅机器人发布的状态消息。ROS 2 Topic 本身是发布/订阅通信通道，不具有固定的请求或响应方向。{arm_side} 取值为 left 或 right。例如 /api/{arm_side}_arm/target_pose 对应 /api/left_arm/target_pose 和 /api/right_arm/target_pose。

4.1.1 状态 Topic 参数

上表为机器人侧标称参数。经 zenoh bridge 到达用户主机后的实际 QoS 和频率，可使用以下命令确认：

ros2 topic info --verbose <TOPIC>
ros2 topic hz <TOPIC>

JointState.header.stamp 是状态聚合节点的发布时间，不是底层电机原始采样时间。单臂关节状态由两路 CAN 数据汇聚，跨关节严格同刻性不作保证。

4.2 机械臂末端控制

Topic： /api/{arm_side}_arm/target_pose；消息类型：geometry_msgs/msg/Pose。position 是末端在对应机械臂肩部坐标系中的位置：左臂使用 left_shoulder_base，右臂使用 right_shoulder_base；orientation 是末端相对该坐标系的姿态，四元数顺序为 [x, y, z, w]。系统接收末端目标并以内置 IK 转换为关节目标。

• 输入中的 position 和 orientation 必须全部为有限数值，四元数不得为全零；当前版本不保证非法目标的处理结果。

• 用户代码需从当前实际位置到目标位置规划平滑路径，并以 50–77 Hz 固定频率依次发布路径点。

• 使用前配置 API 模式、对应手臂为末端控制模式且启用；同时持续发送使能心跳。

• 首次测试先读当前末端位姿、原样回发一次，再小幅修改目标。不要一次发送与当前差距很大的目标。

# 终端 A：保持运行
ros2 topic pub -r 20 /api/fsm/enable std_msgs/msg/Float32 "{data: 1.0}"

# 终端 B：读取当前位姿
ros2 topic echo --once /right_arm/current_ee_pose

# 将读到的 position、orientation 原样填入
ros2 topic pub -r 50 /api/right_arm/target_pose geometry_msgs/msg/Pose \
"{position: {x: <当前x>, y: <当前y>, z: <当前z>}, \
orientation: {x: <当前x>, y: <当前y>, z: <当前z>, w: <当前w>}}"

预期：机械臂就绪后保持当前位姿；将 z 小幅修改（例如 +0.02 m）时，末端缓慢移动到新位置。

4.3 机械臂关节控制

Topic： /api/{arm_side}_arm/joint_cmd；消息类型：sensor_msgs/msg/JointState。

• 仅 position 字段生效，填入 7 维期望关节角，单位为 rad；velocity、effort 会被忽略。

• 用户接管关节空间目标规划，目标序列须经过安全验证。

• 需以 50 Hz 及以上持续发布，发布间隔必须小于 0.15 s；停发后机械臂因收不到新指令而自动停住。

• 使用前配置 API 模式、对应手臂为关节控制模式且启用；同时持续发送使能心跳。

# 终端 A：保持运行
ros2 topic pub -r 20 /api/fsm/enable std_msgs/msg/Float32 "{data: 1.0}"

# 终端 B：读取当前关节角
ros2 topic echo --once /right_arm/joint_states

# 将 name 与 position 原样填入，velocity、effort 留空
ros2 topic pub -r 50 /api/right_arm/joint_cmd sensor_msgs/msg/JointState \
"{name: [<照抄上一步的关节名>], position: [<照抄上一步的7个角度值>], velocity: [], effort: []}"

预期：机械臂就绪后保持当前姿态；将某一个角度小幅改动（约 0.1 rad），对应关节缓慢运动至新目标。

4.4 夹爪控制

Topic： /api/{arm_side}_gripper/cmd；消息类型：std_msgs/msg/Float32。data 是夹爪电机前馈力矩的归一化输入，不是开合位置、开合宽度或末端夹持力。合法范围为 [0.0, 1.0]：数值越小越偏向张开方向，数值越大越偏向合爪方向；抓取通常使用 0.6–1.0。

4.1.1 夹爪输入与前馈力矩映射

表中数值仅为电机前馈力矩；实际输出还受电机位置和速度反馈影响，不能据此换算夹爪开合宽度或末端夹持力。0.10 附近的前馈力矩较小，具体输入应根据物体和实机测试选择。输入必须是 [0.0, 1.0] 范围内的有限数值，禁止发送 NaN、Inf 或越界值。当前版本不保证非法输入的处理结果。夹爪命令会保持最后一次有效输入。停止发布夹爪命令不会自动松爪；只要对应机械臂的控制指令流持续，最后一次夹爪命令会继续生效。对应机械臂控制流中断后，底层约 100 ms 进入看门狗状态并取消夹爪前馈力矩。因此使用夹爪时，机械臂控制指令的发布间隔必须小于 100 ms，建议继续以 50 Hz 发布。夹爪状态见 /{arm_side}_gripper/joint_states。其中 position 是电机角，不是开合宽度；effort 是电机力矩，不是末端夹持力。

4.4.2 夹爪运动学分析

夹爪自由度为1，来自于中心的电机旋转。设广义坐标为q，对应夹爪电机转角，当电机向着夹爪张开的方向运动时，\dot{q}>0。

如图所示（仅展示左夹爪分支，右侧原理相同），所有坐标系的z 轴均垂直于纸面向外。定义\{b\}是基坐标系，固定不动，\{0\}是转子坐标系、随着电机旋转，由q 决定其姿态，即

R_0^b=R_z(-q)

由于机构约束较为复杂，我们引入辅助角\alpha=\left[\varphi,\theta\right]^T ，这两个角分别定义了图中\{0\}\to\{1\} 和\{2\}\to\{2'\} 的坐标系变换，即

\begin{aligned} R_1^0=&R_z(\varphi)\\ R_{2'}^2=&R_z(\theta) \end{aligned}

夹爪闭合时，\{b\},\{0\},\{1\},\{2\} 均具有相同的姿态，\{1\},\{2\} 是固连于所在杆件的。\{2\},\{2'\} 的原点重合，但\{2'\} 是固连于下侧平面的杆件的——y轴沿v字短柄的方向。

正运动学描述q,\alpha 到夹爪末端水平位置 x\in\mathbb{R}^2 的映射，i.e. x=F(q,\alpha) 。建立增广系统并使用隐式微分求取雅各比矩阵

J=\frac{\partial{F}}{\partial{q}}-\frac{\partial{F}}{\partial{\alpha}}\left(\frac{\partial{g}}{\partial{\alpha}}\right)^{-1}\frac{\partial{g}}{\partial{q}}

其中g(q,\alpha)=0 是光滑约束。

若电机旋转速度与力矩\dot{q},\tau（均为输出端）已知，速度与力变换关系为

\begin{aligned} \dot{x}=&J\dot{q}\\ \tau=&J^Tf \end{aligned}

其中f\in\mathbb{R}^2 是夹爪末端所受水平力。

由于自由度为1，我们只能分析单方向上的受力，夹爪末端的力主要由开合方向上的正压力贡献，设开合方向的方向向量为n\in\mathbb{R}^2，与J同在基坐标系下描述，力f 在n 上的投影大小为\lambda\in\mathbb{R}，那么：

\begin{aligned} \tau=&J_l^Tf_l+J_r^Tf_r\\ =&\lambda_l J_l^Tn_l+\lambda_r J_r^Tn_r\\ \end{aligned}

在无法获得左右独立接触力的情况下，暂假设两侧法向力大小相等(i.e. \lambda_l=\lambda_r=\lambda)；该假设不成立时，单个电机力矩无法唯一确定左右两侧力。
由于夹爪部件受平行四边形机构约束、在运动过程中不发生旋转，\vec{n}应为一个常量、左右夹爪相反。则两侧法向力大小为：

\lambda=\frac{\tau}{J_l^Tn_l+J_r^Tn_r}

4.5 底盘控制

Topic： /api/chassis/velocity；消息类型：std_msgs/msg/Float32MultiArray。data=[vx, vy, wz]，其中 vx、vy 为底盘 x、y 方向线速度，单位为 m/s；wz 为绕 z 轴角速度，单位为 rad/s。正负号分别表示沿对应坐标轴的正、反方向运动。输入必须包含 3 个有限数值；长度错误可能导致底盘控制节点退出，NaN/Inf 可能使速度状态失效。当前 API 不做归一化映射或最大速度限幅，用户应根据现场安全条件发送经过验证的小速度目标。默认线速度加速度不超过 0.3 m/s²、减速度不超过 0.6 m/s²；角速度分量使用相同数值，单位为 rad/s²。当前未配置 jerk 限制。底盘不依赖 /api/fsm/enable 心跳。速度指令建议持续发布；停发后底盘保持最后目标，超过 1 s 才将目标清零并受控减速。发送 [0.0, 0.0, 0.0] 会立即将目标设为零，随后按减速度限制停车，并非机械制动意义上的瞬时停止。

ros2 topic pub -r 10 /api/chassis/velocity std_msgs/msg/Float32MultiArray \
  "{data: [0.05, 0.0, 0.0]}"

预期：底盘以约 0.05 m/s 沿 x 轴正方向运动。发送 [0.0, 0.0, 0.2] 时绕 z 轴正方向旋转；坐标轴的实际物理方向应在首次测试时以小速度确认。测试结束时发送全零数组并等待底盘受控停车。/chassis/joint_states 返回 3 个底盘电机的状态，不能直接作为机器人笛卡尔速度或里程计使用。

4.6 升降伺服控制（仅升降版）

Topic： /api/servo/cmd；消息类型：std_msgs/msg/Float32MultiArray；数据为 [normalized_velocity, enable]。

• normalized_velocity：归一化速度输入，范围为 [-1.0, 1.0]；正值上升，负值下降，0.0 停止。

• 速度映射：电机目标速度为 -normalized_velocity × 150 rad/s。因此上升对应电机负转速，下降对应正转速。

• enable：大于 0.5 使能，小于等于 0.5 失能；失能时控制器先发送零速度，再切断伺服使能。

• 接近软件位置限位时自动降低目标速度；到达限位后禁止继续向限位外运动。未收到位置反馈时禁止运动。

• 升降机构不依赖 /api/fsm/enable 心跳；FSM 进入 ERROR 时会受控停车。运动期间建议以 10 Hz 或更高频率持续发布；停发超过 3 s 后受控减速，停止后自动失能并等待新指令。停止时也可显式发送 [0.0, 1.0]，结束控制时发送 [0.0, 0.0]。

• 系统配置中未启用升降伺服时，接口指令不会下发。输入必须包含至少两个有限数值。示例程序会将速度输入限制到 [-1.0,1.0]；用户程序也应执行相同检查。

ros2 topic pub -r 20 /api/servo/cmd std_msgs/msg/Float32MultiArray \
  "{data: [0.3, 1.0]}"

预期：[0.3, 1.0] 上升，[-0.3, 1.0] 下降，[0.0, 1.0] 停止并保持使能，[0.0, 0.0] 停止并失能。

4.7 状态机使能与心跳

Topic： /api/fsm/enable；消息类型：std_msgs/msg/Float32。

• data=1.0：使能/心跳。机器人从暂停状态进入缓启动，并在关节与目标的偏差稳定后进入就绪状态；只有就绪后才会真正跟随手臂指令。

• data=0.0：失能/暂停，立即停止跟随指令。

• 必须以高于 1 Hz 持续发送 1.0，建议 10–20 Hz。超过 1 秒未收到 1.0 会自动暂停。

ros2 topic pub -r 20 /api/fsm/enable std_msgs/msg/Float32 "{data: 1.0}"

4.8 状态反馈

JointState 字段结构与样例

/{arm_side}_arm/joint_states、/{arm_side}_gripper/joint_states 和 /chassis/joint_states 均使用 sensor_msgs/msg/JointState，包含以下字段：

std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

string[] name
float64[] position
float64[] velocity
float64[] effort

以下为实机采集的状态消息，用于说明字段、单位和命名格式。状态数值随机器人姿态和负载变化，不作为标准姿态或验收基准。

机械臂关节状态样例（左臂）

header:
  stamp:
    sec: 1784019894
    nanosec: 898864727
  frame_id: left_arm
name:
- l_joint1
- l_joint2
- l_joint3
- l_joint4
- l_joint5
- l_joint6
- l_joint7
position:
- 0.6281642913818359
- 1.2304353713989258
- -0.8568053245544434
- 0.97955322265625
- 1.116501808166504
- 0.3784332275390625
- 0.5717735290527344
velocity:
- -0.046921730041503906
- 0.0785074234008789
- -0.24750137329101562
- 0.5203323364257812
- 0.7365226745605469
- 0.23414993286132812
- -0.8716392517089844
effort:
- 11.471725463867188
- 0.9686431884765625
- 2.859233856201172
- 5.0968170166015625
- -0.18806838989257812
- 0.000213623046875
- 0.6299839019775391

position 为当前关节角，单位 rad；velocity 单位 rad/s；effort 单位 N·m。name、frame_id 和数组长度必须与实际部件一致。

夹爪关节状态样例（右夹爪）

header:
  stamp:
    sec: 1784020350
    nanosec: 39713212
  frame_id: right_gripper
name:
- r_joint8
position:
- 1.1303119659423828
velocity:
- 0.009613037109375
effort:
- -0.058442115783691406

底盘关节状态样例

header:
  stamp:
    sec: 1784019922
    nanosec: 41234715
  frame_id: chassis
name:
- chassis_joint1
- chassis_joint2
- chassis_joint3
position:
- 2.21746826171875
- -1.838468074798584
- -0.6089911460876465
velocity:
- -0.01861572265625
- 0.00213623046875
- -0.03204345703125
effort:
- -0.12909317016601562
- 0.5410842895507812
- -0.2993812561035156

状态时间语义

JointState.header.stamp 是状态聚合节点的发布时间，不保证等于电机原始采样时刻。单臂关节状态由两路 CAN 数据汇聚，跨关节严格同刻性不作保证。/{arm_side}_arm/current_ee_pose 使用对应机械臂的肩部坐标系，左臂为 left_shoulder_base，右臂为 right_shoulder_base。该消息类型为 geometry_msgs/msg/Pose，不包含 Header；需要时间对齐时，应由接收端记录接收时间。

4.9 CAN 与 MUX Topic

系统包含 /canX/error_code、/canX/motor_cmd、/canX/motor_states、/canX/set_zero 及 /mux/* 等内部 Topic，不属于公开接口。开发者仅使用 /api/* 控制 Topic 和本章列出的状态反馈 Topic，不应向内部 Topic 发布消息。

4.10 控制指令异常行为

各控制接口的正常输入、频率和停止方式见 §4.2–§4.7。本节仅汇总机械臂接口的异常处理：

关节目标推荐以 50 Hz 或更高频率发布，0.15 s 是最大允许单帧间隔，两者不是同一个指标。PAUSE 和 ERROR 下，机械臂持续接收零速度指令并保持电机使能；这不是位置伺服保持、机械抱闸或电机卸力。停发 /api/fsm/enable 需等待约 1 s 超时后进入 PAUSE，主动发布 data=0.0 则立即进入 PAUSE。当前版本不提供多控制程序仲裁、指令序列号、丢包重传、奇异位形状态或指令拒绝原因 Topic。同一时间只允许一个控制程序，见 §3.3。

4.11 运行状态接口

4.11.1 FSM 状态

/fsm_state 使用 std_msgs/msg/Int32，标称发布频率为 20 Hz：

4.11.2 运行模式与部件配置

/api/current_mode 使用 std_msgs/msg/String，以约 1 Hz 发布 JSON：

该 Topic 反映模式和配置，不表示各部件已经 READY，也不包含物理急停、碰撞触发或故障原因。

4.12 状态与传感器能力边界

当前公开状态接口不包含物理急停、碰撞触发、故障原因或指令拒绝原因。未列出的状态与传感器能力当前不对外提供。

4.13 关节限位信息

以下数组按机械臂 7 个关节的顺序排列，单位为 rad。目标关节角必须位于对应机械臂的 lower 与 upper 范围内。

关节限位只描述目标角的范围，不替代碰撞检测、轨迹规划和现场安全检查。发送关节目标前仍需从当前状态平滑起步，并确认机器人周围无人员和障碍物。

05 视觉、数据与模型实践

5.1 RTP/H.265 图像流

• 传输方式：RTP 传输 H.265 视频流，payload type 默认 96。

• 默认接收端口：8890。

• 画面由 4 路相机拼接为一张大图（头左，头右，左腕，右腕）；解码后可固定裁剪为 6 路子画面：头部左右目 2 路，左右腕各左右目 4 路。

• 画面拼接示意图：

• 发送端实际帧率约 45 fps。

5.2 Python 接收接口

rtp_video_interface.py 提供 RTPH265VideoInterface，用于接收一路 RTP/H.265 拼接流并输出 RGB 图像（numpy.ndarray）。它内部维护一张完整拼接图和六张裁剪分割图。下表尺寸为 numpy 数组形状 (高 × 宽 × 通道)：

基本用法

from rtp_video_interface import RTPH265VideoInterface

interface = RTPH265VideoInterface(port=8890)
interface.start()
try:
    if not interface.wait_for_initial_data(timeout=10.0):
        raise RuntimeError("No RTP video frame received")

    observation = interface.get_observation()
    images = observation["images"]
    head_camera = images["head_camera"]
    head_right_eye = images["head_right_eye"]
finally:
    interface.stop()

运行前提：脚本需与 rtp_video_interface.py 放在同一目录（或该目录在 PYTHONPATH 中），且发送端已向本机 8890 端口推流。收到第一帧后，wait_for_initial_data 返回 True；images 字典应包含上表 7 个键，每个值为对应尺寸的 RGB numpy.ndarray。若 10 秒内未收到帧，返回 False，应检查推流、端口、payload 是否为 96。

主要方法

5.3 首帧与录制测试

现有 test.py 可接收 RTP 流、解码并保存六路分割视频。发送端向本机默认 8890 端口推流，且 test.py 与 rtp_video_interface.py 位于同一目录时，可运行：

python3 test.py \
  --port 8890 \
  --split-output-dir ./teleavatar_split \
  --duration-s 10 \
  --log-interval-s 1

预期：日志先输出 Initial video frame received，随后输出帧数、RTP 包数和 FPS；目录生成 6 个 MP4。若等待首帧超时，检查发送端是否推流、端口是否一致、RTP payload 是否为 96、本机是否有可用 H.265 解码器。

5.4 数据采集、下载与转换

当前二次开发数据采集使用 Zebra 版。Web 端录制时，“录制 Topics”为必填项。录制过程中，如果摄像头、机械臂或夹爪状态等必发数据掉线或停止推流，系统将持续播放错误提示音；磁盘使用率达到停止阈值 90% 时，系统自动停止录制以防止文件损坏。录制数据可在 Web 端“录制记录”页面下载，也可通过 SCP 下载。数据目录为 /robotdata/data/recordbags，每个录制任务对应一个子目录。数据转换工具：rosbag_to_dataset_TA2。模型部署参考：openpi。具体的数据采集和上传内容，参考机器人用户手册。

5.5 时间戳、相机标定与多模态同步

1. 1. 所有相机使用一致的外部触发信号，以确保同步触发；
2. 1. 四路相机分别对应 4 个 mailbox，每个 mailbox 只保留最新一帧；新帧覆盖旧帧并计数；
3. 1. 四路分别弹出一帧后，以每路图像的硬件触发时间戳 trig_tv（ms）进行判断；
4. 1. • min_trig_ms == max_trig_ms：判定同步并执行拼接；
   2. • min_trig_ms != max_trig_ms：判定不同步，只丢弃时间戳等于 min_trig_ms 的最旧一路，并计入 ts_drop；其余 mailbox 保留较新帧，等待下一帧后再次比较。

1. 1. 相机内参分为左目内参 KL 和右目内参 KR，数据依次为 fx、fy、cx、cy（单位：像素）。其中 fx、fy 表示焦距，cx、cy 表示主点坐标。
2. 1. 相机标定使用 OpenCV fisheye 模型，具体参见 OpenCV 官方文档。
3. 1. 单个相机的外参由 R | T 构成，表示左目相机到右目相机的外参变换。其中 R 为旋转矩阵，T 为平移向量（单位：mm）。此外，提供立体校正矩阵 RRawL_Row 和 RRawR_Row，用于校正左右目镜头的光轴朝向。

1. 头部相机、机械臂 Base 与腕部相机的坐标系定义及外参变换链详见附录 A。由名义机械结构确定的 virtual_base_T_head_camera_center、virtual_base_T_left_base_virtual 和 virtual_base_T_right_base_virtual 在同型号且机械结构不变时应保持一致；head_camera_center_T_head_left_camera、head_camera_center_T_head_right_camera、left_ee_T_left_wrist_camera 和 right_ee_T_right_wrist_camera 属于单机标定固定变换，不同设备不可直接互换。

1. 像素格式分为相机 CMOS 输出流和落盘数据流。相机 CMOS 输出流为 YUV NV12 格式；落盘的 ROS 2 topic 数据为 H.265 格式。

1. 相机原始数据和落盘数据均未进行校正，需要使用相机内外参进行相应校正。

06 示例与代码

本章每个示例均为可独立运行的 Python 节点。通用运行步骤：

1. 在「系统配置」中按该示例要求选择模式并启用对应部件（详见 2.3），点击 保存配置 → 生效。

1. 将示例代码保存为 .py 文件（如 demo.py）。

1. 设置与本体一致的通信域，再运行：

export ROS_DOMAIN_ID=29
python3 demo.py

每个示例下方均给出运行前提（需要的模式/部件）与预期效果（运行后应观察到的现象）。若现象与描述不符，请先检查模式配置、部件是否启用、话题是否能被发现。

6.1 末端模式示例

• 运行前提：末端控制模式，启用左臂。

• 预期效果：FSM 进入 READY 后，左臂末端从当前位置平滑移动到示例目标；到达后日志打印 Target reached! ...，发送 data=0.0 并进入 PAUSE。该状态表示机械臂零速停住，不是位置伺服保持。

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Pose
from std_msgs.msg import Float32, Int32
import numpy as np

FSM_READY = 2

def quaternion_slerp(q0, q1, t, shortest=True):
    q0 = np.array(q0, dtype=np.float64)
    q1 = np.array(q1, dtype=np.float64)

    q0 /= np.linalg.norm(q0)
    q1 /= np.linalg.norm(q1)

    dot = np.dot(q0, q1)
    if shortest and dot < 0.0:
        q1 = -q1
        dot = -dot
    dot = np.clip(dot, -1.0, 1.0)

    if np.abs(dot - 1.0) < 1e-6:
        return q0.tolist()

    theta = np.arccos(dot) * t
    sin_theta = np.sin(np.arccos(dot))
    q_rot = q0 * np.cos(theta) + (q1 - q0 * dot) * np.sin(theta) / sin_theta
    return q_rot.tolist()

class SinglePointMoveNode(Node):
    def __init__(self):
        super().__init__('single_point_move_node')

        # 发布器
        self.left_pose_pub = self.create_publisher(Pose, '/api/left_arm/target_pose', 10)
        self.enable_pub = self.create_publisher(Float32, '/api/fsm/enable', 10)

        # 订阅当前末端位姿
        self.left_pose_sub = self.create_subscription(
            Pose, '/left_arm/current_ee_pose', self.left_pose_callback, 10)
        self.fsm_state_sub = self.create_subscription(
            Int32, '/fsm_state', self.fsm_state_callback, 10)

        self.enable_timer = self.create_timer(0.05, self.enable_callback)
        self.traj_timer = self.create_timer(0.02, self.timer_callback)

        # 目标位姿（用户定义）
        self.target_pose = Pose()
        self.target_pose.position.x = -0.014433992985935767
        self.target_pose.position.y = 0.0181230355198717
        self.target_pose.position.z = -0.545
        self.target_pose.orientation.x = -0.651863775688076
        self.target_pose.orientation.y = 0.260180077341881
        self.target_pose.orientation.z = 0.7053938323707012
        self.target_pose.orientation.w = 0.0989923560353721

        self.current_left = None
        self.fsm_state = None
        self.traj_points = []   
        self.traj_index = 0   
        self.traj_active = False  
        self.plan_done = False  
        self.reached = False   

        self.pos_tolerance = 0.005  
        self.quat_dot_tolerance = 0.99  

    def enable_callback(self):
        msg = Float32()
        msg.data = 1.0
        self.enable_pub.publish(msg)

    def fsm_state_callback(self, msg):
        self.fsm_state = msg.data

    def left_pose_callback(self, msg):
        self.current_left = msg

        if not self.plan_done and self.current_left is not None:
            self.generate_trajectory()
            self.plan_done = True

        if not self.reached and self.current_left is not None:
            self.check_reached()

    def generate_trajectory(self):
        start = self.current_left
        p_start = [start.position.x, start.position.y, start.position.z]
        q_start = [start.orientation.x, start.orientation.y,
                   start.orientation.z, start.orientation.w]

        p_target = [self.target_pose.position.x, self.target_pose.position.y,
                    self.target_pose.position.z]
        q_target = [self.target_pose.orientation.x, self.target_pose.orientation.y,
                    self.target_pose.orientation.z, self.target_pose.orientation.w]

        # 根据直线距离决定插值步数（至少 20 步）
        dist = np.linalg.norm(np.array(p_target) - np.array(p_start))
        steps = max(int(dist / 0.01), 20)

        self.traj_points = []
        for i in range(steps + 1):
            t = i / steps

            px = p_start[0] + (p_target[0] - p_start[0]) * t
            py = p_start[1] + (p_target[1] - p_start[1]) * t
            pz = p_start[2] + (p_target[2] - p_start[2]) * t

            q_interp = quaternion_slerp(q_start, q_target, t)

            pose = Pose()
            pose.position.x = px
            pose.position.y = py
            pose.position.z = pz
            pose.orientation.x = q_interp[0]
            pose.orientation.y = q_interp[1]
            pose.orientation.z = q_interp[2]
            pose.orientation.w = q_interp[3]

            self.traj_points.append(pose)

        self.traj_index = 0
        self.traj_active = True
        self.get_logger().info(f"Trajectory generated with {len(self.traj_points)} points")

    def timer_callback(self):
        if self.fsm_state != FSM_READY or not self.traj_active:
            return

        if self.traj_index < len(self.traj_points):
            self.left_pose_pub.publish(self.traj_points[self.traj_index])
            self.traj_index += 1
        else:
            self.traj_active = False
            self.get_logger().info("All trajectory points published, waiting for actual arrival...")

    def check_reached(self):
        """检查是否已到达目标位姿"""
        dx = self.current_left.position.x - self.target_pose.position.x
        dy = self.current_left.position.y - self.target_pose.position.y
        dz = self.current_left.position.z - self.target_pose.position.z
        pos_err = np.sqrt(dx*dx + dy*dy + dz*dz)

        q_curr = [self.current_left.orientation.x, self.current_left.orientation.y,
                  self.current_left.orientation.z, self.current_left.orientation.w]
        q_tar = [self.target_pose.orientation.x, self.target_pose.orientation.y,
                 self.target_pose.orientation.z, self.target_pose.orientation.w]

        q_curr = np.array(q_curr) / np.linalg.norm(q_curr)
        q_tar = np.array(q_tar) / np.linalg.norm(q_tar)
        dot = abs(np.dot(q_curr, q_tar))

        if pos_err < self.pos_tolerance and dot > self.quat_dot_tolerance:
            self.reached = True
            self.traj_active = False
            # 停止轨迹发布定时器
            self.traj_timer.cancel()
            # 停止 enable 定时器，并发送一次禁用信号
            if hasattr(self, 'enable_timer'):
                self.enable_timer.cancel()
            disable_msg = Float32()
            disable_msg.data = 0.0
            self.enable_pub.publish(disable_msg)
            self.get_logger().info(f"Target reached! Position error = {pos_err:.4f} m, "
                                   f"Quaternion dot = {dot:.4f}. Timer stopped.")

def main(args=None):
    rclpy.init(args=args)
    node = SinglePointMoveNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        disable_msg = Float32()
        disable_msg.data = 0.0
        node.enable_pub.publish(disable_msg)
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

6.2 夹爪控制示例（基于关节模式）

• 运行前提：关节控制模式，启用左右臂，并持续发布对应的机械臂关节目标和 FSM 心跳。

• 预期效果：双臂持续回发当前关节角，保持机械臂控制流；同时向双爪发送 0.04，即张开方向的前馈力矩输入（不是开合位置）。可修改该值观察不同力方向输入的效果。

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
from sensor_msgs.msg import JointState

class ApiDualGripperJointNode(Node):
    def __init__(self):
        super().__init__('api_dual_gripper_joint_test')
  
  
        # Gripper Publishers
        self.left_gripper_pub = self.create_publisher(Float32, '/api/left_gripper/cmd', 10)
        self.right_gripper_pub = self.create_publisher(Float32, '/api/right_gripper/cmd', 10)

        self.left_joint_cmd_pub = self.create_publisher(JointState, '/api/left_arm/joint_cmd', 10)
        self.right_joint_cmd_pub = self.create_publisher(JointState, '/api/right_arm/joint_cmd', 10)

        # 持续发布 FSM 心跳和机械臂保持指令，使手臂控制流保持有效。
        # 夹爪指令会随对应手臂控制流下发。
        self.enable_pub = self.create_publisher(Float32, '/api/fsm/enable', 10)
        self.create_timer(0.05, self.enable_callback)  # 20Hz 心跳
  
        self.create_subscription(
            JointState,
            "/left_arm/joint_states",
            self.left_joint_state_callback,
            10
        )
        self.create_subscription(
            JointState,
            "/right_arm/joint_states",
            self.right_joint_state_callback,
            10
        )
        self.current_left_joint_state = None
        self.current_right_joint_state = None
  
        # Timers
        self.create_timer(0.02, self.timer_callback) # 50Hz
  
        self.get_logger().info("API Dual Gripper Joint Test Node Started")

  
    def left_joint_state_callback(self, msg):
        self.current_left_joint_state = msg

    def right_joint_state_callback(self, msg):
        self.current_right_joint_state = msg

    def enable_callback(self):
        msg = Float32()
        msg.data = 1.0
        self.enable_pub.publish(msg)

    def timer_callback(self):
        current_time = self.get_clock().now()
  
        # Publish Gripper Command
        gripper_msg = Float32()
        # 0.04：张开方向前馈力矩输入，不是开合位置
        gripper_msg.data = 0.04
  
        if self.current_left_joint_state is None or self.current_right_joint_state is None:
            self.get_logger().warn("Waiting for joint state data...")
            return
  
        left_joint_cmd = JointState()
        left_joint_cmd.header.stamp = current_time.to_msg()  
        left_joint_cmd.name = self.current_left_joint_state.name
        left_joint_cmd.position = self.current_left_joint_state.position
        left_joint_cmd.velocity = [0.0] * len(self.current_left_joint_state.name)  
        left_joint_cmd.effort = [0.0] * len(self.current_left_joint_state.name)
  
        right_joint_cmd = JointState()
        right_joint_cmd.header.stamp = current_time.to_msg()
        right_joint_cmd.name = self.current_right_joint_state.name
        right_joint_cmd.position = self.current_right_joint_state.position
        right_joint_cmd.velocity = [0.0] * len(self.current_right_joint_state.name)
        right_joint_cmd.effort = [0.0] * len(self.current_right_joint_state.name)
  
        self.left_joint_cmd_pub.publish(left_joint_cmd)
        self.right_joint_cmd_pub.publish(right_joint_cmd)
  
        self.left_gripper_pub.publish(gripper_msg)
        self.right_gripper_pub.publish(gripper_msg)

def main(args=None):
    rclpy.init(args=args)
    node = ApiDualGripperJointNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

6.3 夹爪控制示例（基于末端模式）

• 运行前提：末端控制模式，启用左右臂，并持续发布对应的末端目标和 FSM 心跳。

• 预期效果：示例锁存首帧当前末端位姿并持续回发，使机械臂控制流保持有效；同时发送夹爪力方向输入。GRIPPER_CMD=0.0 表示最大张开方向前馈力矩，1.0 表示最大合爪方向前馈力矩，二者都不是开合位置。

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32
from geometry_msgs.msg import Pose

GRIPPER_CMD = 0.0      # 最大张开方向前馈力矩；1.0 为最大合爪方向，均不是位置
ENABLE_FSM = True      # True：本示例持续发布 FSM 心跳；False：仅停止本示例的心跳发布

class ApiDualGripperEEHoldNode(Node):
    def __init__(self):
        super().__init__('api_dual_gripper_ee_hold_test')

        # Gripper Publishers
        self.left_gripper_pub = self.create_publisher(Float32, '/api/left_gripper/cmd', 10)
        self.right_gripper_pub = self.create_publisher(Float32, '/api/right_gripper/cmd', 10)

        # Target pose Publishers (subscribed by mux in cartesian mode)
        self.left_pose_pub = self.create_publisher(Pose, '/api/left_arm/target_pose', 10)
        self.right_pose_pub = self.create_publisher(Pose, '/api/right_arm/target_pose', 10)

        # FSM enable
        self.enable_pub = self.create_publisher(Float32, '/api/fsm/enable', 10)

        # Subscribe to current EE pose (FK output)
        self.create_subscription(
            Pose,
            "/left_arm/current_ee_pose",
            self.left_pose_callback,
            10
        )
        self.create_subscription(
            Pose,
            "/right_arm/current_ee_pose",
            self.right_pose_callback,
            10
        )
        self.current_left_pose = None
        self.current_right_pose = None

        # Timers
        self.create_timer(0.02, self.timer_callback)  # 50 Hz 控制流
        self.create_timer(0.05, self.enable_callback)  # 20 Hz FSM 心跳

        self.get_logger().info("API Dual Gripper EE-Hold Test Node Started")

    def left_pose_callback(self, msg):
        # Latch only the first frame (snapshot) to avoid a current->target feedback loop
        if self.current_left_pose is None:
            self.current_left_pose = msg

    def right_pose_callback(self, msg):
        if self.current_right_pose is None:
            self.current_right_pose = msg

    def enable_callback(self):
        if not ENABLE_FSM:
            return
        enable_msg = Float32()
        enable_msg.data = 1.0
        self.enable_pub.publish(enable_msg)

    def timer_callback(self):
        if self.current_left_pose is None or self.current_right_pose is None:
            self.get_logger().warn("Waiting for current_ee_pose data...")
            return

        # Publish Gripper Command
        gripper_msg = Float32()
        gripper_msg.data = GRIPPER_CMD

        # 持续回发锁存位姿，使手臂控制流保持有效
        self.left_pose_pub.publish(self.current_left_pose)
        self.right_pose_pub.publish(self.current_right_pose)

        # Publish gripper command
        self.left_gripper_pub.publish(gripper_msg)
        self.right_gripper_pub.publish(gripper_msg)

def main(args=None):
    rclpy.init(args=args)
    node = ApiDualGripperEEHoldNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        if ENABLE_FSM:
            disable_msg = Float32()
            disable_msg.data = 0.0
            node.enable_pub.publish(disable_msg)
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

6.4 关节模式示例

• 运行前提：关节控制模式，启用左右臂。

• 预期效果：双臂经缓启动后运动到示例设定的对称目标构型（左臂 [0, 1.57, 0, 0.26, 0, 0, 0]、右臂 [0, -1.57, 0, -0.26, 0, 0, 0]）并保持。

• 说明：本示例直接发送固定目标构型。若当前姿态与目标相差较大，建议改用 §6.6 的插值方式起步。左右臂第 4 关节目标距 §4.13 对应软件限位均仅 0.01 rad，修改目标前必须先检查限位。

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from std_msgs.msg import Float32
import numpy as np

class ApiDualArmJointNode(Node):
    def __init__(self):
        super().__init__('api_dual_arm_joint_test')
  
        # Publishers
        self.left_joint_cmd_pub = self.create_publisher(JointState, '/api/left_arm/joint_cmd', 10)
        self.right_joint_cmd_pub = self.create_publisher(JointState, '/api/right_arm/joint_cmd', 10)
  
        # Enable signal
        self.enable_pub = self.create_publisher(Float32, '/api/fsm/enable', 10)
  
        # Subscribers
        self.create_subscription(
            JointState,
            "/left_arm/joint_states",
            self.left_joint_state_callback,
            10
        )
        self.create_subscription(
            JointState,
            "/right_arm/joint_states",
            self.right_joint_state_callback,
            10
        )
  
        self.current_left_joint_state = None
        self.current_right_joint_state = None
  
        # Target values
        # Left: [0, 1.57, 0, 0.26, 0, 0, 0]
        # Right: [0, -1.57, 0, -0.26, 0, 0, 0] (Symmetric)
        self.target_left_position = np.array([0, 1.57, 0, 0.26, 0, 0, 0])
        self.target_right_position = np.array([0, -1.57, 0, -0.26, 0, 0, 0])
  
        # Timers
        self.create_timer(0.05, self.enable_callback)
        self.create_timer(0.02, self.timer_callback)
  
        self.get_logger().info("API Dual Arm Joint Test Node Started")

    def enable_callback(self):
        msg = Float32()
        msg.data = 1.0
        self.enable_pub.publish(msg)

    def left_joint_state_callback(self, msg):
        self.current_left_joint_state = msg

    def right_joint_state_callback(self, msg):
        self.current_right_joint_state = msg

    def timer_callback(self):
        # Process Left Arm
        if self.current_left_joint_state is not None:
            self._process_arm(
                self.current_left_joint_state, 
                self.target_left_position, 
                self.left_joint_cmd_pub, 
                "Left"
            )
        else:
            self.get_logger().warning("Waiting for left joint states...", throttle_duration_sec=1.0)

        # Process Right Arm
        if self.current_right_joint_state is not None:
            self._process_arm(
                self.current_right_joint_state, 
                self.target_right_position, 
                self.right_joint_cmd_pub, 
                "Right"
            )
        else:
            self.get_logger().warning("Waiting for right joint states...", throttle_duration_sec=1.0)

    def _process_arm(self, current_state, target_pos, pub, arm_name):
        current_pos = np.array(current_state.position)
  
        if len(current_pos) != len(target_pos):
            self.get_logger().warning(f"{arm_name} dimension mismatch: {len(current_pos)} vs {len(target_pos)}", throttle_duration_sec=1.0)
            return
  
        # position = 期望关节角；velocity 由平台自动计算，这里留 0。
        msg = JointState()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.name = current_state.name
        msg.position = target_pos.tolist()
        msg.velocity = [0.0] * len(target_pos)
        msg.effort = [0.0] * len(target_pos)
  
        pub.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    node = ApiDualArmJointNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

6.5 底盘控制示例

• 运行前提：启用底盘；底盘控制不需要 FSM 心跳。

• 预期效果：底盘以 0.05 m/s 沿 x 轴正方向运动约 2 秒，随后发送全零目标并受控停车。首次运行应以小速度确认坐标轴的实际物理方向。

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32MultiArray

class ChassisVelocityPublisher(Node):
    """
    底盘速度命令发布节点，持续运行约 2 秒后发送零速度并退出。
    """

    def __init__(self):
        super().__init__('chassis_velocity_api_publisher')

        self.chassis_vel_pub = self.create_publisher(
            Float32MultiArray,
            '/api/chassis/velocity',
            10
        )

        # 发布频率 10Hz
        self.publish_rate = 10.0
        self.timer = self.create_timer(1.0 / self.publish_rate, self.publish_velocity_cmd)

        # 速度命令（初始为 0）
        self.linear_x = 0.0
        self.linear_y = 0.0
        self.wz = 0.0

        # 2秒后自动停止的定时器
        self.shutdown_timer = self.create_timer(2.0, self.shutdown_callback)

        self.get_logger().info('Chassis velocity publisher started (will run for 2 seconds)')


    def publish_velocity_cmd(self):
        """定时发布速度命令"""
        msg = Float32MultiArray()
        msg.data = [self.linear_x, self.linear_y, self.wz]
        self.chassis_vel_pub.publish(msg)
        self.get_logger().info(
            f'Published: linear_x={self.linear_x:.2f}, '
            f'linear_y={self.linear_y:.2f}, wz={self.wz:.2f}'
        )

    def shutdown_callback(self):
        """2秒后停止：发布零速度并关闭节点"""
        self.get_logger().info('Test finished, stopping robot.')

        # 发布零速度确保停止
        stop_msg = Float32MultiArray()
        stop_msg.data = [0.0, 0.0, 0.0]
        self.chassis_vel_pub.publish(stop_msg)

        # 取消所有定时器
        self.timer.cancel()
        self.shutdown_timer.cancel()

        # 设置标志，让主循环退出
        self.shutdown_flag = True

    def set_velocity(self, linear_x, linear_y, wz):
        """设置底盘线速度（m/s）和角速度（rad/s）"""
        self.linear_x = linear_x
        self.linear_y = linear_y
        self.wz = wz


def main(args=None):
    rclpy.init(args=args)

    # 创建节点
    chassis_publisher = ChassisVelocityPublisher()

    # 设置微小直线速度（沿 x 轴正方向 0.05 m/s）
    chassis_publisher.set_velocity(
        linear_x=0.05,
        linear_y=0.0,
        wz=0.0
    )

    # 自定义主循环，检测退出标志
    try:
        while rclpy.ok() and not hasattr(chassis_publisher, 'shutdown_flag'):
            rclpy.spin_once(chassis_publisher, timeout_sec=0.1)
    except KeyboardInterrupt:
        chassis_publisher.get_logger().info('Interrupted by user')
    finally:
        # 确保最后发布零速度
        stop_msg = Float32MultiArray()
        stop_msg.data = [0.0, 0.0, 0.0]
        chassis_publisher.chassis_vel_pub.publish(stop_msg)

        chassis_publisher.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

6.6 初始位置示例（基于关节模式）

• 运行前提：关节控制模式，启用左右臂。

• 预期效果：示例锁存左右臂首帧关节状态，并在 5 秒内插值到目标姿态；两臂稳定收敛后打印 Both arms converged!，发送失能指令并退出。

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from std_msgs.msg import Float32
import numpy as np

TARGETS = {
    "right_arm": np.array([-0.50, -0.92, 0.52, -1.28, 0.32, 0.55, -0.52]),
    "left_arm": np.array([0.41, 1.16, -0.47, 0.90, 0.23, -0.15, 0.60]),
}

class InitialPositionNode(Node):
    def __init__(self):
        super().__init__("initial_position_demo")
        self.joint_publishers = {
            side: self.create_publisher(JointState, f"/api/{side}/joint_cmd", 10)
            for side in TARGETS
        }
        self.enable_pub = self.create_publisher(Float32, "/api/fsm/enable", 10)

        self.current = {side: None for side in TARGETS}
        self.start = {side: None for side in TARGETS}
        self.names = {side: [] for side in TARGETS}
        for side in TARGETS:
            self.create_subscription(
                JointState,
                f"/{side}/joint_states",
                lambda msg, arm=side: self.state_callback(arm, msg),
                10,
            )

        self.start_time = None
        self.converged_since = None
        self.finished = False
        self.duration = 5.0
        self.tolerance = 0.05
        self.create_timer(0.05, self.publish_enable)  # 20 Hz
        self.create_timer(0.01, self.publish_trajectory)  # 100 Hz

    def state_callback(self, side, msg):
        if len(msg.position) != 7:
            return
        position = np.array(msg.position, dtype=float)
        self.current[side] = position
        self.names[side] = list(msg.name)
        if self.start[side] is None:
            self.start[side] = position.copy()

    def publish_enable(self):
        if not self.finished:
            self.enable_pub.publish(Float32(data=1.0))

    def publish_trajectory(self):
        if self.finished or any(self.start[side] is None for side in TARGETS):
            return

        now = self.get_clock().now()
        if self.start_time is None:
            self.start_time = now
            self.get_logger().info("Initial joint states latched; starting interpolation")

        elapsed = (now - self.start_time).nanoseconds / 1e9
        alpha = min(elapsed / self.duration, 1.0)

        for side, target in TARGETS.items():
            command = self.start[side] * (1.0 - alpha) + target * alpha
            msg = JointState()
            msg.header.stamp = now.to_msg()
            msg.name = self.names[side]
            msg.position = command.tolist()
            msg.velocity = [0.0] * 7
            msg.effort = [0.0] * 7
            self.joint_publishers[side].publish(msg)

        if alpha < 1.0:
            return

        max_error = max(
            np.max(np.abs(self.current[side] - target))
            for side, target in TARGETS.items()
        )
        if max_error < self.tolerance:
            if self.converged_since is None:
                self.converged_since = now
            elif (now - self.converged_since).nanoseconds / 1e9 >= 0.5:
                self.finished = True
                self.get_logger().info("Both arms converged!")
        else:
            self.converged_since = None

    def disable(self):
        self.enable_pub.publish(Float32(data=0.0))

def main(args=None):
    rclpy.init(args=args)
    node = InitialPositionNode()
    try:
        while rclpy.ok() and not node.finished:
            rclpy.spin_once(node, timeout_sec=0.1)
    except KeyboardInterrupt:
        pass
    finally:
        node.disable()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == "__main__":
    main()

目标数组顺序遵循 §4.0，且位于 §4.13 的软件限位内。修改目标前应逐项检查限位，并保持从当前状态平滑起步。

6.7 升降电机示例（仅适用于升降版）

• 运行前提：启用升降伺服（仅升降版机型）。

• 预期效果：升降机构先上升约 1.5 秒 → 暂停约 0.4 秒 → 下降约 1.5 秒 → 停止并失能；日志依次打印 Lift up、Pause、Lift down、Stop。

#!/usr/bin/env python3
import time
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32MultiArray

class LiftServoApiDemo(Node):
    def __init__(self):
        super().__init__("lift_servo_api_demo")
        self.servo_command_pub = self.create_publisher(
            Float32MultiArray,
            "/api/servo/cmd",
            10,
        )

    def publish_servo_command(self, normalized_velocity: float, servo_enabled: bool):
        normalized_velocity = max(-1.0, min(1.0, float(normalized_velocity)))
        if not servo_enabled:
            normalized_velocity = 0.0
        msg = Float32MultiArray()
        msg.data = [
            normalized_velocity,          # 归一化速度指令 [-1, 1]
            1.0 if servo_enabled else 0.0, # 使能标志
        ]
        self.servo_command_pub.publish(msg)

    def publish_for_duration(
        self,
        normalized_velocity: float,
        servo_enabled: bool,
        duration_sec: float,
        rate_hz: float = 20.0,
    ):
        dt = 1.0 / rate_hz
        end_time = time.time() + duration_sec
        while rclpy.ok() and time.time() < end_time:
            self.publish_servo_command(normalized_velocity, servo_enabled)
            rclpy.spin_once(self, timeout_sec=0.0)
            time.sleep(dt)

    def stop_and_disable(self):
        self.publish_servo_command(0.0, False)

    def run_demo(self):
        self.get_logger().info("Lift up")
        self.publish_for_duration(
            normalized_velocity=0.30,
            servo_enabled=True,
            duration_sec=1.5,
        )
        self.get_logger().info("Pause")
        self.publish_for_duration(
            normalized_velocity=0.0,
            servo_enabled=True,
            duration_sec=0.4,
        )
        self.get_logger().info("Lift down")
        self.publish_for_duration(
            normalized_velocity=-0.30,
            servo_enabled=True,
            duration_sec=1.5,
        )
        self.get_logger().info("Stop")
        self.stop_and_disable()

def main(args=None):
    rclpy.init(args=args)
    node = LiftServoApiDemo()
    try:
        node.run_demo()
    except KeyboardInterrupt:
        pass
    finally:
        node.stop_and_disable()
        node.destroy_node()
        rclpy.shutdown()

if __name__ == "__main__":
    main()

6.8 图像接收录制脚本

• 运行前提：完成 §3.2 的图像接收依赖安装，机器人已向用户主机 8890/udp 推流。

• 预期效果：收到首帧后持续记录 6 路裁剪画面，并在指定时长结束或收到退出信号时关闭文件。默认按发送端约 45 fps 封装；实际输入帧率不同时应通过 --output-fps 调整。完整测试方法见 §5.3。

#!/usr/bin/env python3
"""
Bring-up test for RTPH265VideoInterface.
This file records decoded RTP/H.265 split frames to MP4 for validation. The
production interface stays in rtp_video_interface.py and does not own
file-writing logic.
"""
from __future__ import annotations
import argparse
import logging
from pathlib import Path
import signal
import threading
import time
import cv2
import numpy as np
try:
    from .rtp_video_interface import RTPH265VideoInterface
except ImportError:
    from rtp_video_interface import RTPH265VideoInterface
class SplitMp4Recorder:
    """Write split interface images to MP4 without coupling to the decoder."""
    def __init__(
        self,
        *,
        split_output_dir: Path,
        output_fps: float,
    ):
        self.split_output_dir = split_output_dir.expanduser()
        self.output_fps = output_fps
        self.split_output_dir.mkdir(parents=True, exist_ok=True)
        self._writers: dict[str, cv2.VideoWriter] = {}
        self._writer_paths: dict[str, Path] = {}
        self._writer_sizes: dict[str, tuple[int, int]] = {}
        self._frame_counts: dict[str, int] = {}
        self._last_timestamps: dict[str, float] = {}
    def write_latest(self, interface: RTPH265VideoInterface) -> None:
        images, timestamps = interface.get_latest_images_with_timestamps()
        for name in interface.split_image_names:
            self._write_if_new(name, self.split_output_dir / f"{name}.mp4", images, timestamps, name)
    def close(self) -> None:
        for key, writer in self._writers.items():
            writer.release()
            logging.info("Wrote %d frames to %s", self._frame_counts[key], self._writer_paths[key])
        self._writers.clear()
    def _write_if_new(
        self,
        key: str,
        path: Path,
        images: dict[str, np.ndarray],
        timestamps: dict[str, float],
        image_name: str,
    ) -> None:
        frame_rgb = images.get(image_name)
        timestamp = timestamps.get(image_name)
        if frame_rgb is None or timestamp is None:
            return
        if self._last_timestamps.get(key) == timestamp:
            return
        writer = self._get_writer(key, path, frame_rgb)
        size = (frame_rgb.shape[1], frame_rgb.shape[0])
        if size != self._writer_sizes[key]:
            logging.warning("Skipping %s frame with changed size: %s != %s", key, size, self._writer_sizes[key])
            return
        writer.write(cv2.cvtColor(frame_rgb, cv2.COLOR_RGB2BGR))
        self._frame_counts[key] += 1
        self._last_timestamps[key] = timestamp
    def _get_writer(self, key: str, path: Path, frame_rgb: np.ndarray) -> cv2.VideoWriter:
        writer = self._writers.get(key)
        if writer is not None:
            return writer
        size = (frame_rgb.shape[1], frame_rgb.shape[0])
        writer = _open_mp4_writer(path, size, self.output_fps)
        self._writers[key] = writer
        self._writer_paths[key] = path
        self._writer_sizes[key] = size
        self._frame_counts[key] = 0
        logging.info("Recording %s to %s at %.2f fps", key, path, self.output_fps)
        return writer
def _open_mp4_writer(path: Path, size: tuple[int, int], fps: float) -> cv2.VideoWriter:
    path.parent.mkdir(parents=True, exist_ok=True)
    writer = cv2.VideoWriter(str(path), cv2.VideoWriter_fourcc(*"mp4v"), fps, size)
    if not writer.isOpened():
        raise RuntimeError(f"Failed to open MP4 writer: {path}")
    return writer
def _parse_args() -> argparse.Namespace:
    parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter)
    parser.add_argument("--camera-name", default="head_camera")
    parser.add_argument("--port", type=int, default=8890)
    parser.add_argument("--payload", type=int, default=96)
    parser.add_argument("--udp-buffer-size", type=int, default=20_000_000)
    parser.add_argument("--decoder", default="nvh265dec max-display-delay=0")
    parser.add_argument("--log-interval-s", type=float, default=2.0)
    parser.add_argument("--initial-timeout-s", type=float, default=10.0)
    parser.add_argument("--duration-s", type=float, help="Record for this many seconds after the first frame")
    parser.add_argument("--output-fps", type=float, default=45.0)
    parser.add_argument(
        "--split-output-dir",
        type=Path,
        required=True,
        help="Write one MP4 per split crop to this directory",
    )
    return parser.parse_args()
def main() -> int:
    args = _parse_args()
    logging.basicConfig(
        level=logging.INFO,
        format="[%(asctime)s] %(levelname)s: %(message)s",
        datefmt="%H:%M:%S",
        force=True,
    )
    interface = RTPH265VideoInterface(
        camera_name=args.camera_name,
        port=args.port,
        payload=args.payload,
        udp_buffer_size=args.udp_buffer_size,
        decoder=args.decoder,
        log_interval_s=args.log_interval_s,
    )
    recorder = SplitMp4Recorder(
        split_output_dir=args.split_output_dir,
        output_fps=args.output_fps,
    )
    stop_requested = threading.Event()
    signal.signal(signal.SIGINT, lambda _signum, _frame: stop_requested.set())
    signal.signal(signal.SIGTERM, lambda _signum, _frame: stop_requested.set())
    if args.duration_s is not None:
        logging.info("Will record for %.1fs after the first decoded frame", args.duration_s)
    exit_code = 0
    interface.start()
    try:
        if not interface.wait_for_initial_data(timeout=args.initial_timeout_s):
            return 1
        start_time = time.monotonic()
        while not stop_requested.is_set():
            recorder.write_latest(interface)
            if args.duration_s is not None and time.monotonic() - start_time >= args.duration_s:
                break
            if interface.wait(timeout=0.002):
                exit_code = 1
                break
    finally:
        recorder.close()
        interface.stop()
    return exit_code
if __name__ == "__main__":
    raise SystemExit(main())

6.9 图像接收接口脚本

• 运行前提：完成 §3.2 的图像接收依赖安装，机器人已向用户主机 8890/udp 推流。

• 预期效果：接口在后台接收一路 RTP/H.265 拼接流，并提供完整图及 6 路裁剪画面的线程安全读取方法。使用方法见 §5.2。

#!/usr/bin/env python3
"""
Receive a TeleAvatar H.265 RTP video stream as RGB images.
This mirrors the image-facing part of TeleavatarROS2Interface: decoded frames
are stored in latest_images under a lock, and get_observation() returns image
copies. The interface keeps both the composite frame and six split camera views.
Test-only file writing lives in test.py.
"""
from __future__ import annotations
from collections import deque
import logging
import threading
import time
from typing import Dict
from typing import Optional
import numpy as np
try:
    import gi
    gi.require_version("Gst", "1.0")
    from gi.repository import GLib
    from gi.repository import Gst
except ImportError as exc:
    raise RuntimeError(
        "PyGObject/GStreamer Python bindings are required. Install python3-gi "
        "and gir1.2-gstreamer-1.0, or run with a Python that has gi available."
    ) from exc
Gst.init(None)
TELEAVATAR_SPLIT_REGIONS: tuple[tuple[str, tuple[float, float, float, float]], ...] = (
    ("head_right_eye", (0.1250, 0.0000, 0.8750, 0.3529411765)),
    ("head_left_eye", (0.1250, 0.3529411765, 0.8750, 0.7058823529)),
    ("right_wrist_left_eye", (0.0000, 0.7058823529, 0.5000, 0.8529411765)),
    ("right_wrist_right_eye", (0.5000, 0.7058823529, 1.0000, 0.8529411765)),
    ("left_wrist_left_eye", (0.0000, 0.8529411765, 0.5000, 1.0000)),
    ("left_wrist_right_eye", (0.5000, 0.8529411765, 1.0000, 1.0000)),
)
class RTPH265VideoInterface:
    """Thread-safe image interface backed by a GStreamer RTP/H.265 decoder."""
    def __init__(
        self,
        *,
        camera_name: str = "head_camera",
        port: int = 8890,
        payload: int = 96,
        udp_buffer_size: int = 20_000_000,
        decoder: str = "nvh265dec max-display-delay=0",
        log_interval_s: float = 2.0,
    ):
        self.camera_name = camera_name
        self.port = port
        self.payload = payload
        self.udp_buffer_size = udp_buffer_size
        self.decoder = decoder
        self.log_interval_s = log_interval_s
        self.split_regions = TELEAVATAR_SPLIT_REGIONS
        self.split_image_names = tuple(name for name, _box in self.split_regions)
        self.logger = logging.getLogger(self.__class__.__name__)
        self.lock = threading.Lock()
        self.latest_images: Dict[str, np.ndarray] = {}
        self.image_timestamps: Dict[str, float] = {}
        self._pipeline: Optional[Gst.Pipeline] = None
        self._loop: Optional[GLib.MainLoop] = None
        self._thread: Optional[threading.Thread] = None
        self._first_frame = threading.Event()
        self._eos_or_error = threading.Event()
        self._frame_count = 0
        self._rtp_packet_count = 0
        self._h265_buffer_count = 0
        self._stats_start_time: Optional[float] = None
        self._last_log_time = time.monotonic()
        self._last_log_frame_count = 0
        self._decode_start_times: deque[float] = deque(maxlen=512)
        self._latest_decode_latency_ms: Optional[float] = None
    def start(self) -> None:
        """Start receiving images in a background GStreamer thread."""
        if self._thread and self._thread.is_alive():
            return
        self._reset_runtime_state()
        pipeline_description = self._build_pipeline()
        self.logger.info("Starting GStreamer pipeline: %s", pipeline_description)
        pipeline = Gst.parse_launch(pipeline_description)
        if not isinstance(pipeline, Gst.Pipeline):
            raise RuntimeError("GStreamer description did not create a Pipeline")
        appsink = pipeline.get_by_name("decoded_frames")
        if appsink is None:
            raise RuntimeError("Failed to find appsink named decoded_frames")
        appsink.connect("new-sample", self._image_callback)
        rtp_probe = pipeline.get_by_name("rtp_probe")
        if rtp_probe is not None:
            rtp_probe.connect("handoff", self._on_rtp_packet)
        h265_probe = pipeline.get_by_name("h265_probe")
        if h265_probe is not None:
            h265_probe.connect("handoff", self._on_h265_buffer)
        bus = pipeline.get_bus()
        bus.add_signal_watch()
        bus.connect("message", self._on_bus_message)
        self._pipeline = pipeline
        self._loop = GLib.MainLoop()
        self._first_frame.clear()
        self._eos_or_error.clear()
        self._thread = threading.Thread(target=self._run_loop, name="rtp-h265-video", daemon=True)
        self._thread.start()
    def stop(self) -> None:
        """Stop receiving images."""
        if self._loop is not None and self._loop.is_running():
            self._loop.quit()
        if self._thread is not None:
            self._thread.join(timeout=2.0)
        if self._pipeline is not None:
            self._pipeline.set_state(Gst.State.NULL)
            self._pipeline = None
        self._loop = None
        self.logger.info("Stopped decoder after %d frames", self._frame_count)
    def wait_for_initial_data(self, timeout: float = 10.0) -> bool:
        """Wait for the first decoded image, matching TeleavatarROS2Interface."""
        self.logger.info("Waiting for initial video frame...")
        if self._first_frame.wait(timeout=timeout):
            self.logger.info("Initial video frame received")
            return True
        self.logger.error(
            "Timeout waiting for video after %.1fs: rtp_packets=%d h265_buffers=%d",
            timeout,
            self._rtp_packet_count,
            self._h265_buffer_count,
        )
        return False
    def get_observation(self) -> Optional[dict]:
        """Return current image observation, or None before the first frame."""
        with self.lock:
            if self.camera_name not in self.latest_images:
                return None
            return {"images": {name: image.copy() for name, image in self.latest_images.items()}}
    def get_latest_image(self) -> Optional[np.ndarray]:
        """Return the latest composite RGB image."""
        with self.lock:
            image = self.latest_images.get(self.camera_name)
            return None if image is None else image.copy()
    def get_latest_images(self) -> Dict[str, np.ndarray]:
        """Return all latest images, including the composite and split crops."""
        with self.lock:
            return {name: image.copy() for name, image in self.latest_images.items()}
    def get_latest_images_with_timestamps(self) -> tuple[Dict[str, np.ndarray], Dict[str, float]]:
        """Return latest image copies and their receive timestamps."""
        with self.lock:
            images = {name: image.copy() for name, image in self.latest_images.items()}
            return images, dict(self.image_timestamps)
    def has_initial_frame(self) -> bool:
        return self._first_frame.is_set()
    def wait(self, timeout: Optional[float] = None) -> bool:
        """Wait for EOS/error. Returns True if the stream ended."""
        return self._eos_or_error.wait(timeout=timeout)
    def _reset_runtime_state(self) -> None:
        self._frame_count = 0
        self._rtp_packet_count = 0
        self._h265_buffer_count = 0
        self._stats_start_time = None
        self._last_log_time = time.monotonic()
        self._last_log_frame_count = 0
        self._decode_start_times.clear()
        self._latest_decode_latency_ms = None
    def _build_pipeline(self) -> str:
        caps = (
            "application/x-rtp,"
            "media=video,"
            "clock-rate=90000,"
            "encoding-name=H265,"
            f"payload={self.payload}"
        )
        decode = (
            f"udpsrc port={self.port} buffer-size={self.udp_buffer_size} caps=\"{caps}\" "
            "! identity name=rtp_probe signal-handoffs=true silent=true "
            "! rtph265depay "
            "! h265parse config-interval=-1 disable-passthrough=true "
            "! video/x-h265,stream-format=byte-stream,alignment=au "
            "! identity name=h265_probe signal-handoffs=true silent=true "
            f"! {self.decoder} "
            "! videoconvert n-threads=4 "
            "! video/x-raw,format=RGB "
        )
        appsink = (
            "queue leaky=downstream max-size-buffers=1 max-size-bytes=0 max-size-time=0 "
            "! appsink name=decoded_frames emit-signals=true max-buffers=1 drop=true sync=false"
        )
        return f"{decode} ! {appsink}"
    def _run_loop(self) -> None:
        if self._pipeline is None or self._loop is None:
            raise RuntimeError("Pipeline was not initialized")
        pipeline = self._pipeline
        loop = self._loop
        if pipeline.set_state(Gst.State.PLAYING) == Gst.StateChangeReturn.FAILURE:
            self.logger.error("Failed to set GStreamer pipeline to PLAYING")
            self._eos_or_error.set()
            return
        try:
            loop.run()
        finally:
            pipeline.set_state(Gst.State.NULL)
    def _image_callback(self, sink: Gst.Element) -> Gst.FlowReturn:
        sample = sink.emit("pull-sample")
        if sample is None:
            return Gst.FlowReturn.ERROR
        decoded_at = time.monotonic()
        try:
            frame = _sample_to_rgb(sample)
        except Exception:
            self.logger.exception("Failed to convert decoded sample to RGB")
            return Gst.FlowReturn.OK
        split_frames = self._split_frame(frame)
        self._update_decode_latency(decoded_at)
        timestamp = time.time()
        with self.lock:
            self.latest_images[self.camera_name] = frame
            self.image_timestamps[self.camera_name] = timestamp
            for name, split_frame in split_frames.items():
                self.latest_images[name] = split_frame
                self.image_timestamps[name] = timestamp
        now = time.monotonic()
        self._frame_count += 1
        self._first_frame.set()
        self._log_stats(frame, now)
        return Gst.FlowReturn.OK
    def _on_rtp_packet(self, _identity: Gst.Element, _buffer: Gst.Buffer) -> None:
        self._rtp_packet_count += 1
    def _on_h265_buffer(self, _identity: Gst.Element, _buffer: Gst.Buffer) -> None:
        self._h265_buffer_count += 1
        self._decode_start_times.append(time.monotonic())
    def _update_decode_latency(self, decoded_at: float) -> None:
        if not self._decode_start_times:
            return
        self._latest_decode_latency_ms = max(0.0, (decoded_at - self._decode_start_times.popleft()) * 1000.0)
    def _split_frame(self, frame: np.ndarray) -> Dict[str, np.ndarray]:
        height, width = frame.shape[:2]
        crops: Dict[str, np.ndarray] = {}
        for name, box in self.split_regions:
            x1, y1, x2, y2 = _normalized_box_to_pixels(box, width, height)
            if x2 > x1 and y2 > y1:
                crops[name] = frame[y1:y2, x1:x2].copy()
        return crops
    def _log_stats(self, frame: np.ndarray, now: float) -> None:
        if self._stats_start_time is None:
            self._stats_start_time = now
            self._last_log_time = now
            self._last_log_frame_count = self._frame_count
            return
        elapsed = now - self._last_log_time
        if elapsed < self.log_interval_s:
            return
        frames_since_last_log = self._frame_count - self._last_log_frame_count
        interval_fps = frames_since_last_log / elapsed if elapsed > 0.0 else 0.0
        overall_elapsed = now - self._stats_start_time
        overall_fps = (self._frame_count - 1) / overall_elapsed if overall_elapsed > 0.0 else 0.0
        self._last_log_time = now
        self._last_log_frame_count = self._frame_count
        latency = "n/a" if self._latest_decode_latency_ms is None else f"{self._latest_decode_latency_ms:.1f} ms"
        self.logger.info(
            "camera=%s frames=%d rtp_packets=%d h265_buffers=%d shape=%s fps=%.2f "
            "overall_fps=%.2f decode_latency=%s",
            self.camera_name,
            self._frame_count,
            self._rtp_packet_count,
            self._h265_buffer_count,
            tuple(frame.shape),
            interval_fps,
            overall_fps,
            latency,
        )
    def _on_bus_message(self, _bus: Gst.Bus, message: Gst.Message) -> None:
        if message.type == Gst.MessageType.ERROR:
            error, debug = message.parse_error()
            self.logger.error("GStreamer error from %s: %s", message.src.get_name(), error)
            if debug:
                self.logger.error("GStreamer debug info: %s", debug)
            self._eos_or_error.set()
            if self._loop is not None:
                self._loop.quit()
        elif message.type == Gst.MessageType.EOS:
            self._eos_or_error.set()
            if self._loop is not None:
                self._loop.quit()

def _sample_to_rgb(sample: Gst.Sample) -> np.ndarray:
    caps = sample.get_caps()
    if caps is None or caps.get_size() == 0:
        raise RuntimeError("Decoded sample has no caps")
    structure = caps.get_structure(0)
    width = int(structure.get_value("width"))
    height = int(structure.get_value("height"))
    if structure.get_value("format") != "RGB":
        raise RuntimeError(f"Expected RGB sample, got {structure.get_value('format')}")
    buffer = sample.get_buffer()
    if buffer is None:
        raise RuntimeError("Decoded sample has no buffer")
    ok, map_info = buffer.map(Gst.MapFlags.READ)
    if not ok:
        raise RuntimeError("Failed to map decoded frame buffer")
    try:
        row_bytes = width * 3
        raw = np.frombuffer(map_info.data, dtype=np.uint8)
        stride = raw.size // height if raw.size % height == 0 and raw.size // height >= row_bytes else row_bytes
        return raw[: height * stride].reshape((height, stride))[:, :row_bytes].reshape((height, width, 3)).copy()
    finally:
        buffer.unmap(map_info)
def _normalized_box_to_pixels(
    box: tuple[float, float, float, float],
    width: int,
    height: int,
) -> tuple[int, int, int, int]:
    x1, y1, x2, y2 = box
    return (
        max(0, min(width, round(x1 * width))),
        max(0, min(height, round(y1 * height))),
        max(0, min(width, round(x2 * width))),
        max(0, min(height, round(y2 * height))),
    )

6.10 MuJoCo 离线开发路径

仓库已提供可独立复制运行的 mujoco/ 示例，目录内包含 URDF、19 个 STL mesh、转换脚本、MJCF 场景、ROS 2 双臂关节仿真器和测试，不依赖仓库外层文件。安装 Python mujoco 包并加载 ROS 2 Humble 环境后即可使用。交互控制测试需要先运行仿真器：

# 终端 1
cd /absolute/path/to/mujoco
export SIM_ROS_DOMAIN_ID=90
./run_sim.sh --viewer

# 终端 2
cd /absolute/path/to/mujoco
export SIM_ROS_DOMAIN_ID=90
python3 test_control.py --reordered-names

test_control.py 会锁存当前双臂状态，平滑执行小幅关节目标，验证 READY 和收敛后返回起点并失能。可通过 --arms、--joints、--amplitude 和 --duration 调整测试；--dry-run 只打印计划，不发布指令。端到端 smoke 会自行启动并清理仿真器，应单独执行：

export SIM_ROS_DOMAIN_ID=90
python3 smoke_test.py

模型生成与单元测试也从该绝对目录执行：

python3 convert_urdf.py --check-only --verbose
python3 -m unittest discover -s tests -v
python3 play.py

ROS 2 仿真器直接公开关节模式接口：订阅 /api/fsm/enable、/api/left_arm/joint_cmd、/api/right_arm/joint_cmd，以 200 Hz 发布双臂 joint_states、20 Hz 发布 /fsm_state，并以 1 Hz 发布 JSON /api/current_mode。最小状态机为 PAUSE=0、SLOW_START=1、READY=2；READY 要求双臂均收到合法命令，且实际 qpos 连续多个 20 Hz 周期接近目标。命令按关节名重排并由 MJCF ctrlrange 限位。run_sim.sh 和 smoke 强制 ROS_LOCALHOST_ONLY=1，默认隔离域为 90，可用 SIM_ROS_DOMAIN_ID 覆盖；两端必须选择同一非 29 域，脚本拒绝实机生产域 29。该路径只覆盖双臂 14 关节位置模式，不提供末端 pose、夹爪、底盘、升降或 IK。仿真会按 JointState.name 重排命令，而实机接口按 §4\.0 的固定数组顺序读取 position；迁移到实机前必须恢复正式关节顺序。仿真参数为名义值，不代表实机标定、安全控制行为或碰撞安全保证。

07 版本与变更记录

附录 A 坐标系与相机外参定义

本附录整合《机器人统一坐标系定义规范》中与头部相机、机械臂 Base 和腕部相机外参直接相关的定义，作为 §5.5 的内部引用依据。内容仅规定坐标系语义、变换方向及参数属性，不涉及具体标定算法。

A.1 变换记号与通用约定

• {}^A T_B 表示坐标系 B 到坐标系 A 的 4 × 4 齐次变换矩阵。

• 点坐标的变换关系为：{}^A p = {}^A T_B · {}^B p。

• 标定文件采用 parent_T_child 命名，矩阵使用行优先布局。

• 平移量单位为毫米，关节角单位为弧度，四元数顺序为 xyzw。

• 所有坐标系均为右手坐标系。读取程序必须按文件中的 conventions 解析，不得依赖程序默认值推断。

A.2 坐标系定义

• virtual_base：机器人唯一公共参考坐标系。原点位于头部双目两个光心连线的中点；x 轴指向机器人前方，y 轴指向机器人左侧，z 轴指向机器人上方。

• head_camera_center：由头部左右相机共同定义的双目中心坐标系，原点与 virtual_base 重合，姿态由双目基线和共同光轴确定。

• head_left_camera / head_right_camera：头部左右真实相机的光学坐标系。

• left_base_virtual / right_base_virtual：左右机械臂在名义机械结构中的安装基座坐标系，相对于 virtual_base 固定。

• left_ee / right_ee：左右机械臂末端坐标系，其位姿随机械臂关节角变化。

• left_wrist_camera / right_wrist_camera：安装在左右机械臂腕部的真实相机光学坐标系。

virtual_base
├── head_camera_center
│   ├── head_left_camera
│   └── head_right_camera
├── left_base_virtual
│   └── left_ee
│       └── left_wrist_camera
└── right_base_virtual
    └── right_ee
        └── right_wrist_camera

A.3 名义固定变换

名义固定变换由机器人型号、机械设计、CAD 或 URDF 决定，与具体设备无关。同型号且机械结构不变时，不同机器人应使用相同的名义参数；单台设备的装配偏差不包含在这些矩阵中。

• virtual_base_T_head_camera_center：头部双目共同光轴相对水平面名义向下倾斜 26°；两个坐标系原点重合，因此该变换不含平移。

• virtual_base_T_left_base_virtual / virtual_base_T_right_base_virtual：左右机械臂名义 Base 相对于 virtual_base 的固定变换。

virtual_base_T_head_camera_center =
[ 0      -0.4384   0.8988    0      ]
[-1       0        0         0      ]
[ 0      -0.8988  -0.4384    0      ]
[ 0       0        0         1      ]

virtual_base_T_left_base_virtual =
[1  0  0  -105.61]
[0  1  0   220.00]
[0  0  1  -193.11]
[0  0  0     1   ]

virtual_base_T_right_base_virtual =
[1  0  0  -105.61]
[0  1  0  -220.00]
[0  0  1  -193.11]
[0  0  0     1   ]

平移量单位：mm

A.4 单机标定固定变换

下列变换由单台机器人实机标定得到，用于描述真实相机安装关系和单台设备装配差异。标定完成后它们作为常量使用，不随实时关节状态变化，但不同设备之间不可直接互换。

• head_camera_center_T_head_left_camera

• head_camera_center_T_head_right_camera

• left_ee_T_left_wrist_camera

• right_ee_T_right_wrist_camera

每个固定变换必须记录 parent_frame、child_frame、source 和 4 × 4 matrix；source 应说明参数来自机械设计、双目标定或机械臂—相机联合标定，以及是否依赖具体设备。

A.5 运行时变换链

机械臂 Base 到末端的变换由正运动学根据实时关节角计算，属于动态变换。头部相机到机械臂 Base、腕部相机到公共坐标系的关系应由名义固定变换、单机标定固定变换和动态变换按方向组合，不得把动态结果作为固定外参保存。

virtual_base_T_head_left_camera =
  virtual_base_T_head_camera_center
  * head_camera_center_T_head_left_camera

left_base_virtual_T_head_left_camera =
  inverse(virtual_base_T_left_base_virtual)
  * virtual_base_T_head_camera_center
  * head_camera_center_T_head_left_camera

virtual_base_T_left_wrist_camera(q_l) =
  virtual_base_T_left_base_virtual
  * left_base_virtual_T_left_ee(q_l)
  * left_ee_T_left_wrist_camera

右侧相机和右机械臂采用相同规则，将 left 替换为 right。

A.6 使用边界

• 名义固定变换可在同型号、同机械结构的设备间共用；若型号或机械结构变更，必须同步更新。

• 单机标定固定变换和优化后的机械臂运动学参数与具体设备相关，禁止跨设备直接复制。

• 标定用于修正实际运动学计算结果，但不得改变坐标系名称、父子关系或对外接口语义。

• 运行时系统应依据实时关节状态组合固定变换、运动学参数和动态变换，构建完整坐标变换链。