智元远征 A3 Ultra　软件开发手册

0. 快速上手与分步操作

本章动作为主、概述从简：先按步骤把环境跑通、让机器人动起来，再深入各能力域。若已熟悉环境，可直接进入 第 3 章。

0.1 环境自检清单

逐条执行，全部通过后再进行运动控制：

# 速测：确认能调通运动控制 RPC
curl -X POST 'http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlActionService/GetAction' \
     -H 'Content-Type: application/json' -d '{}'
# 期望输出里出现 "current_action" 字段，即链路正常

0.2 分阶推进路线

按序推进，每一阶都留一个可验证的收尾动作：

0.3 端到端示例：语音互动机器人

三模块分别验证，再串成主循环：

1. 播报警示语：调 PlayTTS（6.1）。
2. 走一段 / 挥手：切 MOTION 后发行走速度（4.2）、控制手臂（4.4）。
3. 用唤醒串起来：订阅 /agent/wakeup（6.4），把 1、2 包进主循环。

完整操作链：

0.4 问题排查操作步骤

排查顺序：连通性 → 状态机(Action) → 占用(motion_player/控制源) → 告警(HDS)。

0.5 安全红线

1. 机器人平台与系统架构

1.1 硬件组成（MDU / HDU / ADU）

智元远征 A3 Ultra 机器人工控机由三个核心计算节点组成，均运行 Ubuntu 24.04，使用 systemd 管理系统服务。掌握各节点分工是二次开发路由调用的前提。

1.2 软件系统与 AimDK 定位

A3 Ultra 出厂预搭载完整运动控制、感知、导航程序，二次开发只需调用高层接口，无需开发底层控制模型。

• 通信底座 AimRT：机器人内部框架，提供 Channel（主题通信）与 RPC（请求响应）两种基本通信方式。
• AimDK（Aim Developer Kit）：面向第三方开发者的统一接口层，将内部 AimRT 能力封装为 HTTP JSON RPC 与 ROS2 Topic 两种通用形式。
• 官方文档：https://open.agibot.com/docs/aimdk/a3-ultra/v3_2/

开发涉及三大能力域

2. 二次开发环境搭建

2.1 网络连接与 SSH

1. 开发机与机器人连接到同一无线网络。
2. 在 AimMaster → 设置 → 无线局域网，获取机器人各节点 IP。
3. 通过 SSH 登录目标节点。

# HDU（音频/交互/资源/技能）
ssh agi@10.42.10.10
# ADU（地图/定位/导航 PNC）
ssh agi@10.42.10.11
# MDU（运动控制 Action/关节）
ssh agi@10.42.10.12

2.2 部署目录约定

二次开发统一部署在 /agibot/ 下，示例使用桌面目录：

mkdir -p /agibot/data/home/agi/Desktop

2.3 Python 虚拟环境

cd /agibot/data/home/agi/Desktop
python3 -m venv mydev
source mydev/bin/activate
# 按需安装 requests / rclpy（若走 ROS2 需在 ROS 环境内）

2.4 ROS2 与依赖环境

使用 ROS2 Topic 接口前需加载机器上的 ROS2 与插件环境：

# 1) 机器人运行时环境
source /agibot/software/v0/entry/env/env.sh
# 2) ROS2 Jazzy 基础
source /opt/ros/jazzy/setup.bash
# 3) ros2_plugin_proto 消息（RosMsgWrapper），需在 SDK 的 prebuilt 目录下执行
source /path/to/prebuilt/ros2_plugin_proto_aarch64/share/ros2_plugin_proto/local_setup.bash

2.5 快速上手流程

3. 接口总览

3.1 HTTP JSON RPC HTTP

低频、多对一调用；语言无关，仅需能发 HTTP 请求。统一格式：

POST http://<node_ip>:<port>/rpc/<Service>/<Method>
Header: Content-Type: application/json
Body   : { "header": {...}, ...业务字段 }

各节点 RPC 端口依据服务而定，常用示例：

3.2 ROS2 Topic / Service ROS2

高频、多对多调用，需支持 ROS2 Jazzy（Fast DDS）。主题有两种消息载体：

3.3 消息序列化（JSON / PB）PROTO

RosMsgWrapper 的 serialization_type 决定 data 编码方式：

• json：可直接解析的 JSON 字符串（常用于行走速度控制）。
• pb：protobuf 字节流，需用 aimdk.protocol.* 中对应消息解析。示例中 data = [bytes([x]) for x in json_str] 为按字节切分，解析时用 b"".join(msg.data) 还原。

# 发送（PB 编码，以腰部为例）
from aimdk.protocol.motion_control.motion.mc_motion_channel_pb2 import MotionControlMoveWaistChannel
cmd = MotionControlMoveWaistChannel()
cmd.header.timestamp.seconds = int(now.timestamp())
cmd.waist_pitch = 0.1
raw = cmd.SerializeToString()
wrapper = RosMsgWrapper()
wrapper.serialization_type = "pb"
wrapper.context = ["aimdk.protocol.MotionControlMoveWaistChannel"]
wrapper.data = [bytes([b]) for b in raw]

3.4 控制源 与 状态机 Action

控制源（control_source） 用于标识请求来源与抢占优先级，由安全模块按来源分配。RequestHeader 中的该字段一般保持默认（AUTO=0）即可，仅在需要从特定来源（如安全模块 ControlSource_SAFE=2）发起时显式指定。A3 Ultra 运动控制内置状态机，通过 Action 表达当前运动模式（详见第 4 章）。

3.5 RequestHeader 完整字段 与 接口鉴权

多数 RPC 请求体首层即为 request_header（或 header），其字段用于链路追踪与控制源标识。依据 common/header.proto：

响应侧 ResponseHeader：code（0=成功，非 0 失败，msg 含原因）、msg、timestamp、trace_id、domin。阻塞式调用另有 blocked / id 字段。

接口鉴权（JWT）

aimdk.protocol.InteractionAuthService/GetInteractionAuthJWT 用于申请交互鉴权 JWT（agent→gateway 方向），返回：

{
  "header": {...}, "auth_jwt_status": 0,   // AuthJWTStatus_SUCCESS=0
  "jwt": "", "expire_at": <毫秒时间戳>
}

3.6 错误码与告警等级对照

依据 common/rpc.proto 的 CommonState：RPC 响应顶层的 code 非 0 即失败，msg 含原因。

链路告警等级（HDS，AlertLevel，1~13）：

4. 上肢与运动控制基础

本章介绍运动控制的基础（Action 状态机）以及上肢各部位的控制方法。下肢控制（行走、逐关节、腰部、增强模式、AVATAR）请见 第 5 章 下肢运控与全身逐关节控制。

4.1 Action 状态机切换 RPC

运动控制模块地址 10.42.10.12:56322，服务 aimdk.protocol.MotionControlActionService。

# 查询当前 Action
curl -X POST 'http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlActionService/GetAction' \
     -H 'Content-Type: application/json' -d '{}'

# 切换 Action —— 先 GetAvailableActions/GetNextActions 取列表，再把列表项塞给 command
# 列表项即 MotionControlActionCommand 对象（其 action 为 MotionControlAction 数字枚举），示例取 MOTION=210
curl -X POST 'http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlActionService/SetAction' \
     -H 'Content-Type: application/json' \
     -d '{
       "header": {"control_source": "ControlSource_SAFE", "trace_id": "user_example", "domin": ""},
       "command": {"action": 210, "ext_action": "MotionControlAction_MOTION"}
     }'
# 注意：PNC 章节用的是另一个服务 McActionService，其 action 才是字符串枚举（如 McAction_USE_EXT_CMD），勿混用

Action 取值语义

4.2 手臂控制 ROS2 Topic

主题 /motion/control/arm_joint_command，类型 sensor_msgs/JointState，仅 MOTION 态。14 个关节固定名称：

left_shoulder_pitch_joint,  left_shoulder_roll_joint, left_shoulder_yaw_joint,
left_elbow_joint,           left_wrist_roll_joint,    left_wrist_pitch_joint,    left_wrist_yaw_joint,
right_shoulder_pitch_joint, right_shoulder_roll_joint,right_shoulder_yaw_joint,
right_elbow_joint,          right_wrist_roll_joint,   right_wrist_pitch_joint,   right_wrist_yaw_joint

msg = JointState()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = "user_McScript"
msg.name = ARM_NAMES        # 上述 14 个名称
msg.position = [0.0]*14
msg.velocity = [0.0]*14
msg.effort   = [0.0]*14

4.3 脖子控制 ROS2 Topic

主题 /motion/control/neck_joint_command，仅 MOTION 态，仅支持 位控。仅 2 个关节：

msg.name = ["head_yaw_joint", "head_pitch_joint"]
msg.position = [0.0, 0.2]
# velocity / effort 无实际作用，但必须设置（空会导致运控 crash）
msg.velocity = [0.0, 0.0]
msg.effort   = [0.0, 0.0]

4.4 手指控制 ROS2 Topic

主题 /motion/control/hand_joint_command，仅 MOTION 态。frame_id 填手部类型（AgiHand 或 O10Hand，缺省 AgiHand）。如 O10 手 20 个关节：

msg.header.frame_id = "O10Hand"
# position 范围 0~2000：0=完全张开，2000=完全并拢
# state 主题 position 范围为 0~4096

4.5 关节状态订阅 ROS2 Topic

以下主题类型为 sensor_msgs/JointState，订阅端 BEST_EFFORT。官方示例 examples/mc/joint_state.py 演示了 arm_joint_state 的订阅，脖子/手指状态 topic 可按同样模式订阅：

# 停止 motion_player
curl -X POST 'http://127.0.0.1:50080/json/stop_app' -H 'content-type:application/json' -d '{"app_name":"motion_player"}'
# 重启 motion_player
curl -X POST 'http://127.0.0.1:50080/json/start_app' -H 'content-type:application/json' -d '{"app_name":"motion_player"}'

4.6 上肢控制注意事项

5. 下肢运控与全身逐关节控制专题

本章集中梳理下肢控制与全身逐关节控制的所有路径、接口、适用场景与前提条件。A3 Ultra 标准配置为 31 DOF（腿 12 + 腰 3 + 头 2 + 臂 14），根据控制方式不同可通过分部位 RPC、分部位 Topic 或 TA 全身通道进行控制。

5.1 下肢控制总览：两条路径

下肢控制有两条完全不同的路径，互斥不可同时使用，请根据场景选择：

5.2 31 DOF 全身关节图谱

A3 Ultra 标准全身布局为 TaJointLayout_BODY_31，共 31 个自由度，按部位分类如下。所有关节名、顺序均与 SDK 中 TaWholeBodyCommand / TaWholeBodyState 定义一致。

记忆口诀：腿 12（左 6 + 右 6）→ 腰 3 → 头 2 → 臂 14（左 7 + 右 7），合计 31。全局顺序与 TaWholeBodyCommand.joint_velocities 一致。

代码示例：Python 关节索引映射

在程序中建议使用索引字典，避免硬编码序号。以下为 31 DOF 完整映射：

# 31 DOF 关节名 → 全局索引 映射字典
# 顺序: leg(12) + waist(3) + head(2) + arm(14)
JOINT_NAME_TO_INDEX = {
    # 左腿 (0-5)
    "left_hip_pitch_joint": 0,
    "left_hip_roll_joint": 1,
    "left_hip_yaw_joint": 2,
    "left_knee_joint": 3,
    "left_ankle_pitch_joint": 4,
    "left_ankle_roll_joint": 5,
    # 右腿 (6-11)
    "right_hip_pitch_joint": 6,
    "right_hip_roll_joint": 7,
    "right_hip_yaw_joint": 8,
    "right_knee_joint": 9,
    "right_ankle_pitch_joint": 10,
    "right_ankle_roll_joint": 11,
    # 腰部 (12-14)
    "waist_yaw_joint": 12,
    "waist_roll_joint": 13,
    "waist_pitch_joint": 14,
    # 头部 (15-16)
    "head_yaw_joint": 15,
    "head_pitch_joint": 16,
    # 左臂 (17-23)
    "left_shoulder_pitch_joint": 17,
    "left_shoulder_roll_joint": 18,
    "left_shoulder_yaw_joint": 19,
    "left_elbow_joint": 20,
    "left_wrist_roll_joint": 21,
    "left_wrist_pitch_joint": 22,
    "left_wrist_yaw_joint": 23,
    # 右臂 (24-30)
    "right_shoulder_pitch_joint": 24,
    "right_shoulder_roll_joint": 25,
    "right_shoulder_yaw_joint": 26,
    "right_elbow_joint": 27,
    "right_wrist_roll_joint": 28,
    "right_wrist_pitch_joint": 29,
    "right_wrist_yaw_joint": 30,
}

# 反向索引：序号 → 关节名
JOINT_INDEX_TO_NAME = {v: k for k, v in JOINT_NAME_TO_INDEX.items()}

# 按部位分组的关节名列表（与 JointCommand 全量下发顺序一致）
LEG_JOINT_NAMES = [
    "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint",
    "left_knee_joint",
    "left_ankle_pitch_joint", "left_ankle_roll_joint",
    "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint",
    "right_knee_joint",
    "right_ankle_pitch_joint", "right_ankle_roll_joint",
]  # 12 个

WAIST_JOINT_NAMES = [
    "waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint",
]  # 3 个

ARM_JOINT_NAMES = [
    "left_shoulder_pitch_joint", "left_shoulder_roll_joint",
    "left_shoulder_yaw_joint", "left_elbow_joint",
    "left_wrist_roll_joint", "left_wrist_pitch_joint",
    "left_wrist_yaw_joint",
    "right_shoulder_pitch_joint", "right_shoulder_roll_joint",
    "right_shoulder_yaw_joint", "right_elbow_joint",
    "right_wrist_roll_joint", "right_wrist_pitch_joint",
    "right_wrist_yaw_joint",
]  # 14 个

使用方法：下发前用 LEG_JOINT_NAMES 按顺序构造 12 条 JointCommand，确保全量、顺序正确。

5.3 路径 A：行走速度控制 ROS2 Topic

主题完整名 /motion/control/locomotion_velocity/pb_:aimdk.protocol.MotionControlLocomotionVelocityChannel。按官方示例用 serialization_type="json"，需持续高频发布（50Hz），停止发布即停。仅 MOTION 态有效。

{"data": {
  "mode": 0,     // LocomotionMode_DEFAULT = 0
  "forward_velocity": 0.1,   // 前进速度，单位 m/s，正=前
  "lateral_velocity": 0.0,   // 横向速度，单位 m/s，正=左
  "angular_velocity": 0.0    // 角速度，单位 rad/s，正=左转
}}

示例 examples/mc/walk.py 以 0.1 m/s 前进做保守测试（实际速度上限由机器人决定），50Hz 定时以 mode=0（DEFAULT）、前进 0.1（比例系数）发布，目标 2s 共 100 次。data 逐字节拆分填入 RosMsgWrapper.data。

5.4 路径 B：逐关节位控（腿部 + 腰部） RPC

服务 aimdk.protocol.MotionControlJointService，RPC 端口 10.42.10.12:56322。允许绕过行走速度通道，逐关节直接下发下肢指令。

方法总览

JointCommand 字段

JointCommand {
  name       = 关节名称
  sequence   = 关节序号 (0-based)
  position   = 关节角度 (弧度) 或位置 (米)
  velocity   = 关节角速度 (rad/s)
  effort     = 关节扭矩
  stiffness  = 刚度 (N·m/rad)
  damping    = 阻尼 (N·m·s/rad)
}

操作步骤

# 腿部
curl -X POST 'http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlJointService/GetLegJointState' \
     -H 'Content-Type: application/json' -d '{}'
# 响应 states[] 含 12 个腿部关节，每个含 name/position/velocity/effort

# 腰部
curl -X POST 'http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlJointService/GetWaistJointState' \
     -H 'Content-Type: application/json' -d '{}'

curl -X POST 'http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlJointService/SetLegJointCommand' \
     -H 'Content-Type: application/json' \
     -d '{
       "header": {"control_source": 0},
       "commands": [
         {"name":"left_hip_pitch_joint","sequence":0,"position":0.10,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
         {"name":"left_hip_roll_joint","sequence":1,"position":0.0,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
         {"name":"left_hip_yaw_joint","sequence":2,"position":0.0,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
         {"name":"left_knee_joint","sequence":3,"position":0.10,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
         {"name":"left_ankle_pitch_joint","sequence":4,"position":0.0,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
         {"name":"left_ankle_roll_joint","sequence":5,"position":0.0,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
         {"name":"right_hip_pitch_joint","sequence":6,"position":0.10,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
         {"name":"right_hip_roll_joint","sequence":7,"position":0.0,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
         {"name":"right_hip_yaw_joint","sequence":8,"position":0.0,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
         {"name":"right_knee_joint","sequence":9,"position":0.10,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
         {"name":"right_ankle_pitch_joint","sequence":10,"position":0.0,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
         {"name":"right_ankle_roll_joint","sequence":11,"position":0.0,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0}
       ]
     }'

curl -X POST 'http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlJointService/SetWaistJointCommand' \
     -H 'Content-Type: application/json' \
     -d '{"header":{"control_source":0},"commands":[
       {"name":"waist_yaw_joint","sequence":0,"position":0.0,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
       {"name":"waist_roll_joint","sequence":1,"position":0.0,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0},
       {"name":"waist_pitch_joint","sequence":2,"position":-0.15,"velocity":0.0,"effort":0.0,"stiffness":0,"damping":0}
     ]}'

关节名以上述固定名为准；程序中请按 sequence 顺序全量下发并持续高频刷新（推荐 50Hz）。

5.5 EXT 增强模式详解

EXT 增强模式（McAction_RL_* 系列）是实现下肢逐关节控制的必要前提。它本质是一组"让渡控制权"的运动模式——把特定身体部位（上肢/腰腿/全身）的控制权从内置策略手中让出来，交给外部程序。

增强模式对比（常用 5 种）

上表列举常用 6 种，完整列表以 GetAvailableActions 返回为准。编号 300~499 为 McAction 枚举区间，其中 400+ 为 RL 增强模式。

如何确认固件支持

curl -X POST 'http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlActionService/GetAvailableActions' \
     -H 'Content-Type: application/json' -d '{}'
# 查看 commands[] 中是否包含 McAction_RL_* 开头的枚举

若返回中只有 MotionControlAction_*（PASSIVE/MOTION/AVATAR 等）而无 McAction_RL_*，说明当前固件未开放增强模式，需升级固件或联系官方解锁。

切换方法

通过 MotionControlActionService/SetAction 的扩展命令桥切入：

curl -X POST 'http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlActionService/SetAction' \
     -H 'Content-Type: application/json' \
     -d '{
       "header": {"control_source": "ControlSource_SAFE"},
       "command": {
         "action": 5000,
         "ext_action": "McAction_RL_WHOLE_BODY_EXT_JOINT_SERVO"
       }
     }'

action=5000 即 MotionControlAction_USE_EXT_CMD（扩展命令桥），ext_action 填目标增强模式名。

切回普通模式

curl -X POST 'http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlActionService/SetAction' \
     -H 'Content-Type: application/json' \
     -d '{
       "header": {"control_source": "ControlSource_SAFE", "trace_id": "user_example", "domin": ""},
       "command": {"action": 210, "ext_action": "MotionControlAction_MOTION"}
     }'
# 切回后，腰腿恢复内置策略自主控制

5.6 腰部控制（两种方式对比）

腰部有两条控制路径，分别对应不同场景：

方式一：move_waist（推荐，MOTION 态可用）

主题：/motion/control/move_waist/pb_:aimdk.protocol.MotionControlMoveWaistChannel，PB 序列化，需 context 指定类型。

# MoveWaistChannel 数据字段（WaistMoveValue，比例值 [-1, 1]，映射到实际运动范围）：
x      = 前后位移 (x)
y      = 左右位移 (y)
z      = 垂直升降 (z)
roll   = 左右倾斜 (roll)
pitch  = 前后俯仰 (pitch)
yaw    = 左右旋转 (yaw)

# 范围（来自 SDK 示例 WAIST_LIMITS，仅供参考，以实际为准）：
# pitch:  [-0.50, 0.50] rad  ≈ [-28.6°, +28.6°]
# roll:   [-0.30, 0.30] rad  ≈ [-17.2°, +17.2°]
# yaw:    [-1.57, 1.57] rad  ≈ [-90°, +90°]
# height: [-0.40, 0.00] m    ≈ 下降 40cm / 平齐

PB 发布示例（Python）：

from aimdk.protocol.motion_control.move_waist_pb2 import MotionControlMoveWaistChannel
from ros2_plugin_proto.msg import RosMsgWrapper

ch = MotionControlMoveWaistChannel()
ch.yaw, ch.roll, ch.pitch, ch.z = 0.0, 0.0, -0.3, -0.1  # 比例值，z 对应高度
raw = ch.SerializeToString()

wrapper = RosMsgWrapper()
wrapper.serialization_type = "pb"
wrapper.context = ["aimdk.protocol.MotionControlMoveWaistChannel"]
wrapper.data = [bytes([b]) for b in raw]
# publish 到 move_waist topic，持续高频刷新

方式二：SetWaistJointCommand（精确逐关节）

见 5.4 步骤 5。需 EXT 增强模式，直接下发 3 个腰部关节的精确角度。

5.7 备选：AVATAR 全身接管（TA 链路）

若固件未开放 EXT 增强模式，但 GetAvailableActions 中有 MotionControlAction_AVATAR，可考虑走 AVATAR 遥操模式接管全身（含腰腿）。AVATAR 由 TA（Teleop Avatar）模块驱动，设计目标是让外部动作输入源（动捕、VR、骨骼流）实时控制机器人全身。

5.7.1 TaWholeBodyCommand 全身指令通道

TA 通过 topic /ta/whole_body_command 以 60Hz 发布全身运动指令，MC 订阅。消息类型为 TaWholeBodyCommandChannel（PB 格式），核心数据 TaWholeBodyCommand 字段如下：

发布方式：构造 TaWholeBodyCommandChannel PB 消息 → SerializeToString() → 封装为 RosMsgWrapper（serialization_type="pb"，context=["aimdk.protocol.TaWholeBodyCommandChannel"]）→ 发布到 /ta/whole_body_command。参考 5.6 中 move_waist 的 PB 发布模式。

代码示例：Python PB 发布全身指令

以下示例构造 TaWholeBodyCommand 并发布到 /ta/whole_body_command topic，风格与 SDK move_waist.py 一致：

#!/usr/bin/env python3
"""
TA 全身指令发布示例
向 /ta/whole_body_command 以 60Hz 发布 TaWholeBodyCommandChannel (PB)

依赖:
    pip install aimdk-protocol ros2-plugin-proto rclpy
"""
from datetime import datetime, timezone
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
from ros2_plugin_proto.msg import RosMsgWrapper

# PB 消息导入
from aimdk.protocol.ta.ta_channel_pb2 import TaWholeBodyCommandChannel
from aimdk.protocol.ta.ta_whole_body_command_pb2 import (
    TaJointLayout,
    TaPelvisPose,
    TaPelvisVelocity,
    TaLegJointCommand,
    TaFootContact,
    TaWaistJointCommand,
    TaHeadJointCommand,
    TaArmJointCommand,
    TaJointVelocities,
)

CONTROL_SOURCE_MANUAL = 1
PUBLISH_RATE = 60  # Hz


def fill_header(header, seq: int) -> None:
    """填充消息头（时间戳、序号、控制源）"""
    now = datetime.now(timezone.utc)
    ts = now.timestamp()
    header.seq = seq
    header.timestamp.seconds = int(ts)
    header.timestamp.nanos = now.microsecond * 1000
    header.timestamp.ms_since_epoch = int(ts * 1000)
    header.control_source = CONTROL_SOURCE_MANUAL


class TaWholeBodyPublisher(Node):
    def __init__(self):
        super().__init__("ta_whole_body_publisher")

        # QoS 与 MC 端订阅保持一致：BEST_EFFORT + KEEP_LAST(10)
        qos = QoSProfile(
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
        )
        self.pub = self.create_publisher(
            RosMsgWrapper,
            "/ta/whole_body_command/pb_3Aaimdk_2Eprotocol_2ETaWholeBodyCommandChannel",
            qos,
        )
        self.seq = 0
        self.timer = self.create_timer(1.0 / PUBLISH_RATE, self._on_timer)
        self.get_logger().info(f"TA 全身指令发布器启动，频率 {PUBLISH_RATE}Hz")

    def _build_command(self) -> TaWholeBodyCommandChannel:
        """构造一帧全身指令（站立基准姿态 + 微幅手臂摆动测试）"""
        ch = TaWholeBodyCommandChannel()
        fill_header(ch.header, self.seq)
        cmd = ch.data

        # --- 关节布局：31 DOF ---
        cmd.joint_layout = TaJointLayout.TaJointLayout_BODY_31

        # --- 骨盆位姿（世界坐标系，单位四元数 + 位置） ---
        cmd.pelvis_pose.quat_wxyz.extend([1.0, 0.0, 0.0, 0.0])  # 初始朝向
        cmd.pelvis_pose.position_xyz.extend([0.0, 0.0, 0.0])    # 原点

        # --- 骨盆速度（局部坐标系） ---
        cmd.pelvis_velocity.linear_xyz.extend([0.0, 0.0, 0.0])
        cmd.pelvis_velocity.angular_xyz.extend([0.0, 0.0, 0.0])

        # --- 腿部 12 DOF（站立姿态，双膝微屈） ---
        knee_bend = 0.05  # rad，约 2.9°
        cmd.leg_command.angles_rad.extend([
            0.0, 0.0, 0.0,  knee_bend, 0.0, 0.0,  # 左腿
            0.0, 0.0, 0.0,  knee_bend, 0.0, 0.0,  # 右腿
        ])

        # --- 足底接触（双脚完全接触 = 支撑相） ---
        cmd.foot_contact.left_contact = 1.0
        cmd.foot_contact.right_contact = 1.0
        cmd.foot_contact.detailed_contacts.extend([1.0] * 16)

        # --- 腰部 3 DOF ---
        cmd.waist_command.angles_rad.extend([0.0, 0.0, 0.0])  # yaw, roll, pitch

        # --- 头部 2 DOF ---
        cmd.head_command.angles_rad.extend([0.0, 0.0])  # yaw, pitch

        # --- 手臂 14 DOF（角度 + 速度 + 刚度 + 阻尼） ---
        cmd.arm_command.angles_rad.extend([0.0] * 14)
        cmd.arm_command.velocities_rad_s.extend([0.0] * 14)
        cmd.arm_command.feedforward_efforts.extend([0.0] * 14)
        cmd.arm_command.stiffness.extend([0.0] * 14)  # 0 = 默认刚度
        cmd.arm_command.damping.extend([0.0] * 14)

        # --- 全身关节速度（31 DOF，用于 MC 前馈） ---
        cmd.joint_velocities.velocities_rad_s.extend([0.0] * 31)

        return ch

    def _on_timer(self):
        ch = self._build_command()
        raw = ch.SerializeToString()

        msg = RosMsgWrapper()
        msg.serialization_type = "pb"
        msg.context = ["aimdk.protocol.TaWholeBodyCommandChannel"]
        msg.data = [bytes([b]) for b in raw]

        self.pub.publish(msg)
        self.seq += 1


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


if __name__ == "__main__":
    main()

5.7.2 TA 接入完整流程

TA 模块运行于 service preset 下，所有功能由外部编排。完整接入需以下步骤：

curl -X POST '<TA节点IP>:<端口>/rpc/aimdk.protocol.TaService/InjectCalibration' \
     -H 'Content-Type: application/json' \
     -d '{
       "header": {"control_source": 0},
       "normalization_scale": 1.0,
       "left_foot_rotation_z_deg": 0.0,
       "left_foot_rotation_y_deg": 0.0,
       "left_foot_rotation_x_deg": 0.0,
       "right_foot_rotation_z_deg": 0.0,
       "right_foot_rotation_y_deg": 0.0,
       "right_foot_rotation_x_deg": 0.0,
       "left_foot_translation_z": 0.0,
       "right_foot_translation_z": 0.0,
       "torso_limb_translation_z": 0.0
     }'

字段说明：normalization_scale 为骨架归一化缩放系数（1.0 = 未标定基准值）；足部旋转/平移偏置为足部补偿参数。具体数值需根据机器人实际标定获取，以上为占位默认值。

curl -X POST '<TA节点IP>:<端口>/rpc/aimdk.protocol.TaService/StartSession' \
     -H 'Content-Type: application/json' -d '{"header": {"control_source": 0}}'

curl -X POST '<TA节点IP>:<端口>/rpc/aimdk.protocol.TaService/SwitchMcAction' \
     -H 'Content-Type: application/json' \
     -d '{
       "header": {"control_source": 0},
       "target": 1,
       "avatar_mode": 0
     }'
# target: TA_MC_ACTION_AVATAR = 1
# avatar_mode: MotionControlAvatarMode_DEFAULT = 0

# source: TA_SOURCE_MOCAP_RETARGET=1 / MOTION_MATCHING=2 / ACTION_PLAYER=3

代码示例：TA 服务 RPC 调用（Python）

以下为 TA 服务 8 步接入流程的 Python 封装（使用 requests 调用 HTTP JSON RPC）：

import requests
import time

# TA 服务地址（请替换为实际 TA 节点 IP 和端口）
TA_RPC_BASE = "http://10.42.10.12:56322/rpc/aimdk.protocol.TaService"
# 注：TA 服务端口可能与 MC 不同，具体以实际部署为准


def ta_rpc(method: str, payload: dict) -> dict:
    """通用 TA RPC 调用封装"""
    url = f"{TA_RPC_BASE}/{method}"
    resp = requests.post(url, json=payload, timeout=5)
    resp.raise_for_status()
    return resp.json()


def ta_full_setup():
    """
    TA 全身遥操完整接入流程（8 步）
    前置条件：机器人已上电、MC 在 MOTION 或 PD_STAND 态
    """
    print("=== Step 1: 注入标定参数 ===")
    calib_resp = ta_rpc("InjectCalibration", {
        "header": {"control_source": 0},
        "normalization_scale": 1.0,
        "left_foot_rotation_z_deg": 0.0,
        "left_foot_rotation_y_deg": 0.0,
        "left_foot_rotation_x_deg": 0.0,
        "right_foot_rotation_z_deg": 0.0,
        "right_foot_rotation_y_deg": 0.0,
        "right_foot_rotation_x_deg": 0.0,
        "left_foot_translation_z": 0.0,
        "right_foot_translation_z": 0.0,
        "torso_limb_translation_z": 0.0,
    })
    print(f"  标定注入结果: code={calib_resp.get('header',{}).get('code')}")

    print("=== Step 2: 启动遥操会话 ===")
    start_resp = ta_rpc("StartSession", {"header": {"control_source": 0}})
    print(f"  会话启动结果: code={start_resp.get('header',{}).get('code')}")
    time.sleep(1.0)  # 等待会话就绪

    print("=== Step 3: 切换 MC 到 AVATAR 模式 ===")
    # target=1 = TA_MC_ACTION_AVATAR, avatar_mode=0 = DEFAULT
    switch_resp = ta_rpc("SwitchMcAction", {
        "header": {"control_source": 0},
        "target": 1,
        "avatar_mode": 0,
    })
    print(f"  切换 AVATAR 结果: code={switch_resp.get('header',{}).get('code')}")
    time.sleep(2.0)  # 等待模式切换完成

    print("=== Step 4: 选择数据源（动捕重定向模式） ===")
    # source=1 = TA_SOURCE_MOCAP_RETARGET
    src_resp = ta_rpc("SelectSource", {
        "header": {"control_source": 0},
        "source": 1,
    })
    print(f"  数据源选择结果: code={src_resp.get('header',{}).get('code')}")

    print("=== Step 5: 设置上半身模式（全身绝对） ===")
    # mode=1 = TA_UPPER_BODY_MODE_FULL_ABSOLUTE
    ubm_resp = ta_rpc("SetUpperBodyMode", {
        "header": {"control_source": 0},
        "mode": 1,
    })
    print(f"  上半身模式结果: code={ubm_resp.get('header',{}).get('code')}")

    print("=== Step 6: 查询服务状态确认 ===")
    status = ta_rpc("GetServiceStatus", {"header": {"control_source": 0}})
    print(f"  当前 MC action: {status.get('mc_current_action')}")
    print(f"  接管状态: {status.get('takeover_state')}")
    print(f"  标定状态: {status.get('calibration_state')}")

    print("=== 接入完成，可以开始发布全身指令了 ===")
    print("提示: 持续发布 /ta/whole_body_command（60Hz） + /ta/upper_body_deadman_command")


if __name__ == "__main__":
    ta_full_setup()

5.7.3 全身复位（WholeBodyReset）

进入 AVATAR 后可播放内置全身复位动作，使机器人回到安全姿态后再切换运动模式：

复位状态机：IDLE → STARTING → RUNNING → SUCCEEDED（成功）/ FAILED / CANCELED / TIMED_OUT。

代码示例：全身复位调用

AVATAR 模式下播放内置复位动作，完成后安全切回 MOTION：

def ta_whole_body_reset_and_exit():
    """播放全身复位动作 → 等待完成 → 切回 MOTION"""
    print("=== 启动全身复位（原地踏步） ===")
    # reset_type=1 = TA_WHOLE_BODY_RESET_TYPE_YUANDITABU
    ta_rpc("StartWholeBodyReset", {
        "header": {"control_source": 0},
        "reset_type": 1,
    })

    # 轮询等待复位完成
    print("等待复位完成...", end="", flush=True)
    timeout = 30  # 最长等 30 秒
    for i in range(timeout * 2):  # 2Hz 轮询
        time.sleep(0.5)
        status = ta_rpc("GetServiceStatus", {"header": {"control_source": 0}})
        reset_state = status.get("whole_body_reset_state", "IDLE")
        safe = status.get("safe_to_switch_motion", False)
        print(f".", end="", flush=True)

        # 复位成功且可安全切换
        if reset_state == "SUCCEEDED" and safe:
            print("\n复位完成，安全切换就绪")
            break

        # 异常状态
        if reset_state in ("FAILED", "CANCELED", "TIMED_OUT"):
            print(f"\n复位异常: {reset_state}")
            return False
    else:
        print("\n复位超时")
        return False

    # 切回 MOTION 模式
    print("=== 切回 MOTION 模式 ===")
    # target=2 = TA_MC_ACTION_MOTION
    ta_rpc("SwitchMcAction", {
        "header": {"control_source": 0},
        "target": 2,
    })
    time.sleep(2.0)
    return True

5.7.4 上半身控制模式（UpperBodyMode）

5.7.5 TA 服务方法速查

5.8 全身关节状态读取

获取机器人当前关节状态有两种方式：RPC 拉取（按部位，适合调试）和 Topic 订阅（全身/分部位，适合实时控制闭环）。

方式一：RPC 拉取（按部位）

服务 MotionControlJointService（MDU:56322）提供 5 个部位的状态查询：

每个 JointState 含字段：name / sequence / position / velocity / effort。

方式二：Topic 订阅

/wbc/whole_body_state 是结构化全身状态消息，含 leg_state / waist_state / head_state / arm_state / pelvis_imu / torso_imu 六部分，每个部位为 repeated JointState。适合需要全身状态闭环的场景。

代码示例 1：RPC 拉取腿部状态（Python）

import requests

MC_JOINT_RPC = "http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlJointService"


def get_leg_joint_state() -> list[dict]:
    """
    读取腿部 12 关节实时状态。

    返回:
        list[dict]，每个 dict 含 name/sequence/position/velocity/effort
    """
    resp = requests.post(
        f"{MC_JOINT_RPC}/GetLegJointState",
        json={},
        timeout=3,
    )
    resp.raise_for_status()
    data = resp.json()
    states = data.get("states", [])

    # 打印格式化结果
    print(f"=== 腿部关节状态（共 {len(states)} 个） ===")
    for s in states:
        print(f"  [{s['sequence']:2d}] {s['name']:30s}  "
              f"pos={s['position']:+.4f} rad  "
              f"vel={s['velocity']:+.4f} rad/s  "
              f"eff={s['effort']:+.4f} N·m")
    return states


def get_all_joint_state() -> dict:
    """一次性读取所有部位关节状态"""
    methods = {
        "leg":   "GetLegJointState",
        "arm":   "GetArmJointState",
        "waist": "GetWaistJointState",
        "neck":  "GetNeckJointState",
    }
    result = {}
    for part, method in methods.items():
        resp = requests.post(f"{MC_JOINT_RPC}/{method}", json={}, timeout=3)
        result[part] = resp.json().get("states", [])
    return result


if __name__ == "__main__":
    get_leg_joint_state()

代码示例 2：订阅 /wbc/whole_body_state（Python ROS2）

#!/usr/bin/env python3
"""
订阅全身关节状态 /wbc/whole_body_state (PB)
"""
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy
from ros2_plugin_proto.msg import RosMsgWrapper

from aimdk.protocol.ta.ta_whole_body_state_pb2 import (
    TaWholeBodyStateChannel,
    TaWholeBodyState,
)


class WholeBodyStateSubscriber(Node):
    def __init__(self):
        super().__init__("whole_body_state_sub")

        qos = QoSProfile(
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
        )
        self.sub = self.create_subscription(
            RosMsgWrapper,
            "/wbc/whole_body_state/pb_3Aaimdk_2Eprotocol_2ETaWholeBodyStateChannel",
            self._on_msg,
            qos,
        )
        self.get_logger().info("等待全身状态消息...")

    def _on_msg(self, msg: RosMsgWrapper):
        # 反序列化 PB 数据
        raw = b"".join(msg.data)
        ch = TaWholeBodyStateChannel()
        ch.ParseFromString(raw)
        state: TaWholeBodyState = ch.data

        # 打印关键信息（示例：只打印腿部+骨盆 IMU）
        leg_count = len(state.leg_state.states)
        knee_left  = state.leg_state.states[3].position   # left_knee
        knee_right = state.leg_state.states[9].position   # right_knee

        pelvis_quat = list(state.pelvis_imu.quat_wxyz[:4])

        self.get_logger().info(
            f"全身状态: 腿={leg_count}关节, "
            f"左膝={knee_left:+.3f}rad, 右膝={knee_right:+.3f}rad, "
            f"骨盆四元数=[{pelvis_quat[0]:.3f}, {pelvis_quat[1]:.3f}, "
            f"{pelvis_quat[2]:.3f}, {pelvis_quat[3]:.3f}]"
        )


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


if __name__ == "__main__":
    main()

TaWholeBodyState 包含六个部位：leg_state / waist_state / head_state / arm_state / pelvis_imu / torso_imu。每个部位的 states 数组中，JointState 含 name / sequence / position / velocity / effort 五个字段。

5.9 控制接口与 Action 模式对应总表

不同控制接口有不同的 Action 模式前提。下表汇总全身各部位的控制接口、所需 Action 模式、以及备注：

代码示例：模式切换完整闭环（MOTION → EXT → 控制 → 回 MOTION）

以下为完整的模式切换 + 控制 + 回退流程示例，可作为开发脚手架：

import requests
import time

MC_ACTION_RPC = "http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlActionService"
MC_JOINT_RPC  = "http://10.42.10.12:56322/rpc/aimdk.protocol.MotionControlJointService"
CONTROL_SOURCE = 0  # ControlSource_SAFE


def get_action() -> str:
    """获取当前 MC Action"""
    resp = requests.post(f"{MC_ACTION_RPC}/GetAction", json={}, timeout=3)
    return resp.json().get("state", {}).get("action", "UNKNOWN")


def set_ext_action(ext_action_name: str) -> bool:
    """
    切入 EXT 增强模式。
    action=5000 = MotionControlAction_USE_EXT_CMD（扩展命令桥）
    """
    resp = requests.post(
        f"{MC_ACTION_RPC}/SetAction",
        json={
            "header": {"control_source": CONTROL_SOURCE,
                       "trace_id": "user_demo", "domin": ""},
            "command": {
                "action": 5000,
                "ext_action": ext_action_name,
            },
        },
        timeout=3,
    )
    # SetAction 返回成功仅表示受理，需二次确认
    if resp.json().get("header", {}).get("code") != 0:
        print(f"SetAction 请求被拒绝: {resp.json()}")
        return False
    return True


def wait_for_action(target_action_keyword: str, timeout: float = 10.0) -> bool:
    """轮询等待 Action 切换完成"""
    deadline = time.time() + timeout
    while time.time() < deadline:
        current = get_action()
        if target_action_keyword in current:
            print(f"  ✓ 已切换到 {current}")
            return True
        time.sleep(0.5)
    print(f"  ✗ 切换超时，当前: {get_action()}")
    return False


def ext_joint_control_demo():
    """
    完整流程：
    MOTION → EXT 全身逐关节 → 控制测试 → 回 MOTION
    """
    # --- Step 1: 确认当前在 MOTION ---
    current = get_action()
    print(f"当前 Action: {current}")
    if "MOTION" not in current:
        print("错误：需先处于 MOTION 模式")
        return

    # --- Step 2: 切入 EXT 增强模式 ---
    ext_mode = "McAction_RL_WHOLE_BODY_EXT_JOINT_SERVO"
    print(f"\n切入 EXT 模式: {ext_mode}")
    if not set_ext_action(ext_mode):
        return
    if not wait_for_action("EXT_JOINT_SERVO", timeout=10):
        print("切换失败，终止")
        return

    # --- Step 3: 控制（示例：双膝微屈 0.05rad） ---
    print("\n下发腿部关节指令（微屈膝测试）")
    knee_bend = 0.05
    positions = [
        0.0, 0.0, 0.0,  knee_bend, 0.0, 0.0,  # 左腿
        0.0, 0.0, 0.0,  knee_bend, 0.0, 0.0,  # 右腿
    ]
    leg_cmd = build_leg_joint_request(positions)  # 见 5.4 辅助函数
    # 转 JSON 格式
    cmd_json = {
        "header": {"control_source": CONTROL_SOURCE},
        "commands": [
            {"name": c["name"], "sequence": c["sequence"],
             "position": c["position"], "velocity": c["velocity"],
             "effort": c["effort"], "stiffness": c["stiffness"],
             "damping": c["damping"]}
            for c in [
                {"name": LEG_JOINT_NAMES[i], "sequence": i,
                 "position": positions[i], "velocity": 0.0,
                 "effort": 0.0, "stiffness": 0.0, "damping": 0.0}
                for i in range(12)
            ]
        ]
    }
    resp = requests.post(
        f"{MC_JOINT_RPC}/SetLegJointCommand",
        json=cmd_json,
        timeout=3,
    )
    print(f"  下发结果: code={resp.json().get('header',{}).get('code')}")

    # 持续发布 3 秒（50Hz = 约 150 帧）
    print("  持续发布 3s...")
    for _ in range(150):
        requests.post(
            f"{MC_JOINT_RPC}/SetLegJointCommand",
            json=cmd_json,
            timeout=3,
        )
        time.sleep(0.02)
    print("  发布完成")

    # --- Step 4: 切回 MOTION ---
    print("\n切回 MOTION 模式")
    resp = requests.post(
        f"{MC_ACTION_RPC}/SetAction",
        json={
            "header": {"control_source": CONTROL_SOURCE,
                       "trace_id": "user_demo", "domin": ""},
            "command": {"action": 210,
                        "ext_action": "MotionControlAction_MOTION"},
        },
        timeout=3,
    )
    if wait_for_action("MOTION", timeout=10):
        print("✓ 已安全回到 MOTION 模式")
    else:
        print("⚠ 回切超时，请手动检查")


if __name__ == "__main__":
    ext_joint_control_demo()

5.10 常见问题与排查

排查顺序口诀：先看 Action 对不对 → 再看模式支不支持 → 最后查命令格不格式。

6. 底盘 / 地图 / 导航

5.1 地图管理 RPC

地图模块地址 10.42.10.11:50807，服务 aimdk.protocol.MappingService 与 aimdk.protocol.LocalizationService。

# 获取地图列表
curl -X POST 'http://10.42.10.11:50807/rpc/aimdk.protocol.MappingService/GetStoredMapNames' \
     -H 'Content-Type: application/json' \
     -d '{"header":{},"command":"MappingCommand_GET_STORED_MAP_NAME"}'

# 开始建图
curl -X POST 'http://10.42.10.11:50807/rpc/aimdk.protocol.MappingService/StartMapping' \
     -H 'Content-Type: application/json' \
     -d '{"header":{},"command":"MappingCommand_START_MAPPING","no_realtime_data":true}'

5.2 定位与重定位 RPC

导航前需定位成功，可全局或手动重定位：

手动重定位结果可用 /slam/localization/odometry 验证。

5.3 规控导航（PNC） RPC

PNC 服务地址 10.42.10.11:53176，服务 aimdk.protocol.PncService。执行前置条件：

• 重定位成功，且任务地图 ID 与重定位地图一致。
• MC 已切换到 McAction_RL_LOCOMOTION_DEFAULT 运动态（PNC 示例经 ADU 的 McActionService/SetAction 切换，地址 10.42.10.11:56322；与 MDU 的MotionControlActionService 是两个不同服务，勿混淆）。

# 规划导航到 2D 位姿
curl -X POST 'http://10.42.10.11:53176/rpc/aimdk.protocol.PncService/PlanningNaviToPose2D' \
     -H 'Content-Type: application/json' \
     -d '{
       "header": {"control_source": 0},
       "task_id": 0,
       "map_id": 1,
       "pose": {"position": {"x":10.0, "y":5.0}, "angle": 3.14159},
       "ackerman_mode": false
     }'
# 前置：确认/切入 RL_LOCOMOTION_DEFAULT（pnc_demo 经 ADU 的 McActionService 操作，非运动状态机服务）
curl -X POST 'http://10.42.10.11:56322/rpc/aimdk.protocol.McActionService/SetAction' \
     -H 'Content-Type: application/json' \
     -d '{"command": {"action": 5000, "ext_action": "McAction_RL_LOCOMOTION_DEFAULT"}}'

导航任务控制

返回状态：IDLE / RUNNING / PAUSED / SUCCESS / FAILED（示例使用 PncServiceState_*）。完整可交互 demo 见 examples/pnc/pnc_demo.py，包含"取地图→取拓扑→选点→切 Action→下发→轮询→取消"全链路。

7. Agent 语音交互

6.1 TTS 语音合成 RPC

服务 aimdk.protocol.TTSService，地址 10.42.10.10:59301。

# 播报一段文本（文本上限约 1024 字节 / ~200 个中英文字符）
curl -X POST 'http://10.42.10.10:59301/rpc/aimdk.protocol.TTSService/PlayTTS' \
     -H 'Content-Type: application/json' \
     -d '{
       "text":"你好，我是智元远征 A3 Ultra",
       "priority_level":"INTERACTION_L6",
       "domain":"example",
       "trace_id":"demo_trace_001",
       "is_interrupted":true
     }'

6.2 音频文件播放 RPC

同服务 PlayMediaFile。支持 16kHz / 16bit / 单声道 PCM，以及带标准 44 字节头 Linear PCM 的 WAV。

curl -X POST 'http://10.42.10.10:59301/rpc/aimdk.protocol.TTSService/PlayMediaFile' \
     -H 'Content-Type: application/json' -d '{"file_name":"/path/to/audio.wav"}'

6.3 播放状态与打断 RPC + Topic

状态查询：RPC GetAudioStatus；实时状态订阅主题 /interaction/tts_status/pb_3Aaimdk_2Eprotocol_2ETTSStatus（PB）。状态枚举：Begin / Playing / End / Stop / Error / InQue / NOTInQue。

打断：RPC StopTTSTraceId，按 trace_id 打断单个播放。

# 订阅 TTS 状态（PB 反序列化）
from aimdk.protocol.interaction.tts_service_pb2 import TTSStatus
raw = b"".join(msg.data)
st = TTSStatus(); st.ParseFromString(raw)

6.4 麦克风与唤醒 RPC + Topic

# 关闭语音（静默）
curl -X POST 'http://10.42.10.10:59301/rpc/aimdk.protocol.AgentControlService/SetVoiceEnable' \
     -H 'Content-Type: application/json' -d '{"enable_voice": false}'

7.5 音频焦点 ROS2 Service

A3 Ultra 通过 Service 串音聚焦权限，申请/释放播放权：

# 申请
ros2 service call /audio_5Fmsgs/srv/RequestAudioFocus audio_msgs/srv/RequestAudioFocus \
  "{focus_requester: {pkg_name: my_app, priority: 6, priority_weight: 1}}"
# 释放
ros2 service call /audio_5Fmsgs/srv/AbandonAudioFocus audio_msgs/srv/AbandonAudioFocus '{}'

流式音频播放主题 /audiohal/audio/playback，入参含 pkg_name / priority / priority_weight / channels / sample_rate / sample_format / coding_format / audio_bytes。

8. 资源管理

服务 aimdk.protocol.ResourceService，地址 10.42.10.10:51049。开放六类资源：动作、表情、音频、技能、地图、创作作品。

# 创建表情资源（emoticon_extra_info 指向 tmp 目录下的文件）
curl -X POST 'http://10.42.10.10:51049/rpc/aimdk.protocol.ResourceService/CreateResource' \
     -H 'Content-Type: application/json' \
     -d '{
       "header": {"control_source": 0},
       "resource": {
         "resource_name":"我的表情",
         "resource_type":"RESOURCE_TYPE_EMOTICON",
         "tags":["测试"],
         "emoticon_extra_info":{
           "emoticon_file_url":"tmp/emoticon.mp4",
           "thumbnail_file_url":"tmp/thumbnail.mp4",
           "cover_file_url":"tmp/cover.png"
         }
       }
     }'

各资源类型示例见 examples/resource_manager/CreateResource.py，涵盖 motion / emotion / audio / offring_work（创作作品，对应枚举 RESOURCE_TYPE_OFFRING_WORK，可组合动作+表情+音频，支持 NormalMotion 与 WholeBodyDance）。

9. 技能播放

8.1 动作播放 RPC

服务 aimdk.protocol.MotionCommandService/SendMotionCommand，地址 10.42.10.12:56444。播放/停止/暂停/下一个均走同一接口，通过字段组合控制：

# 播放一个 .mcap 动作
curl -X POST 'http://10.42.10.12:56444/rpc/aimdk.protocol.MotionCommandService/SendMotionCommand' \
     -H 'Content-Type: application/json' \
     -d '{
       "motion_id":"/agibot/data/resources/default/motion/演讲10s/演讲10s.mcap",
       "duration_ms":10000,
       "cmd_end":true, "cmd_pause":false, "cmd_reset":false
     }'

# 停止动作（motion_id 置空，cmd_reset=true）
{"motion_id":"","duration_ms":10000,"cmd_end":true,"cmd_pause":false,"cmd_reset":true}

# 播放列表下一个（motion_id="next_motion"）

8.2 表情播放 ROS2 Topic

主题 /skill/pilot/face/play/pb_3Aaimdk_2Eprotocol_2EHFAEmoction，PB 载荷。字段含 e_path / e_id / repeat / priority / is_stop：

emotion = HFAEmoction()
emotion.e_path = "/agibot/data/resources/default/emoticon/disable_voice/emoticon.mp4"
emotion.e_id = 15
emotion.repeat = 1
emotion.priority = 440
emotion.is_stop = False

8.3 舞蹈 RPC

服务 aimdk.protocol.SkillPilotService/SkillPackage，地址 10.42.10.12:52893。V3.2 新增舞蹈播放：

curl -X POST 'http://10.42.10.12:52893/rpc/aimdk.protocol.SkillPilotService/SkillPackage' \
     -H 'Content-Type: application/json' \
     -d '{
       "header": {"timestamp": {"seconds":"0","nanos":0,"ms_since_epoch":"0"}},
       "source":"custom",
       "command":"Start",
       "path":"/agibot/data/resources/default/skill/查尔斯顿舞",
       "session_id":"12333"
     }'

另提供 SkillPilotService/AutoCharging（自主充电）等接口，接口入参与返回字段以上述 skill_play 示例为准。

10. 系统状态与故障诊断

10.1 告警查询 RPC

服务 aimdk.protocol.HDSService，地址 10.42.10.12:50587。推荐主接口 GetAlertList（0.2Hz 内查询）。

curl -X POST 'http://10.42.10.12:50587/rpc/aimdk.protocol.HDSService/GetAlertList' \
     -H 'Content-Type: application/json' -d '{}'

其他：GetTotalAlertList、GetAlertCount、GetExceptionEvent（较冗杂，优先 GetAlertList）。

10.2 BMS 电池状态 ROS2 Topic

主题 /aima/bms/data/pb_3Aaimdk_2Eprotocol_2EBmsStateChannel，PB 载荷，订阅端 BEST_EFFORT。常用关键字段：voltage / current / power / temperature / capacity / charge / cycles_num / power_supply_status / charger_state / bms_state（毫、微、十分位缩放需按要求换算，见示例 examples/other/bms.py）。

扩展字段还包括：ver / power_supply_health / cycles_capacity / abnormal_state / max_current / battery_firmware_type / battery_pack_state / battery_comm_state / charger_output_state / battery_full_state 等，具体以 BmsState proto 定义为准。

10.3 急停与系统状态（含复位） Topic

急停主题 /hal_state/emergency/pb_3Aaimdk_2Eprotocol_2EEmergencyStateChannel。系统模式包含 EStop：其 active 列表含 EStop + Motion，触发时 deactivate mc、activate motion_player。系统模式/工作模式相关 Topic 见 aimdk.protocol.hal.state.* 与 mc.base.work_mode。

急停复位完整流程

触发急停后（物理急停按钮 / SystemService.TriggerEStop），MC 进入 deactivate、机器人不可运动。恢复前先确认危险已排除，再按序复位：

10.4 音频音量 RPC

服务 aimdk.protocol.HalAudioService，地址 10.42.10.10:56666：GetAudioVolume / SetAudioVolume。另同服务 PlayFile/StopPlay 播放 /agibot/data/var/hal_audio/file/ 下音频（WAV 自带格式头；PCM 需传 channels 与 samplerate）。

11. 高级能力域（感知 / 具身 / 多机 / 系统管理）

本章覆盖运动/语音/导航之外的进阶能力：环境感知、具身操作（机械臂抓取 / 任务引擎）、多机群组（linkswarm）与系统管理（设置 / OTA / 紧急模式）。这些协议子包在 SDK protocol/protobuf 下真实存在，但多数没有官方可运行示例，字段以后文为准、接口参数以对应模块实际发布格式为准。

10.1 环境感知（perception / camera / sensors）

感知协议位于 aimdk.protocol.perception.*，相机与传感器位于 aimdk.protocol.hal.camera.* / hal.sensors.*。

感知层（perception/perception_service.proto）

目标类型枚举 TargetType：DETECT_UNKNOWN / PLASTIC_BOX / METAL_SHELF；目标点类型 TargetPointType：TPT_UNKNOWN / TPT_COARSE_POINT / TPT_FINE_POINT。

相机与实时图像（hal/camera & hal/sensors）

图像类型枚举 CameraImageType：UNKNOWN / COLOR / DEPTH / IR；单目模型 CameraModel：UNKNOWN / PINHOLE / FISHEYE / CMEI。实时流通过 image_channel.proto / color_depth_image_channel.proto 提供的主题订阅。

图像流订阅（Topic）

实时图像流通过 PB 主题发布，消息定义见 image_channel.proto（彩色）与 color_depth_image_channel.proto（彩色+深度），订阅方式与第 4 章其他 PB Topic 一致：

# 以 ROS2 Python 为例（与 4.7 move_waist 相同模式）
from aimdk.protocol.hal.sensors.image_channel_pb2 import ImageChannel
from ros2_plugin_proto.msg import RosMsgWrapper

msg = RosMsgWrapper()
msg.serialization_type = "pb"
msg.context = ["aimdk.protocol.ImageChannel"]
# data 字段为 PB 序列化字节，逐字节填入
# 收到后 ImageChannel.FromString(b''.join(msg.data)) 反序列化

ImageChannel 含 header + 图像 image（像素数据、宽、高、编码、步长）等字段；ColorDepthImageChannel 同时含彩色与深度两路。相机型号（PINHOLE/FISHEYE/CMEI）与内参见 GetCameraIntrinsicsState。

10.2 具身操作（manipulation / task_engine）

机械臂抓取操作（manipulation/manipulation_service.proto）

服务 aimdk.protocol.ManipulationService，端口 10.42.10.本地:39110。

任务引擎（task_engine/）

调度多步具身任务：

• TaskEngineService：GetTask / GetAllTasks / GetAllFSMs / SetTask / SetCurrentTask / DeleteTask / DeleteTaskMap / LaunchTask / CtrlTaskState
• TaskWorkerService：CtrlTaskWorker（type, task_data）→ CtrlTaskWorkerResponse；CheckoutTask（task_name, task_config, is_file）；GetTaskWorkerState
• 标定类：TaskCalibrationService（CalibrationStart/Stop/GetCalibrationResult）、TaskFactoryCalibrationService、TaskRobotCalibrationService、TaskCalibrationValidService

10.3 多机 / 群组（linkswarm）

协议位于 aimdk.protocol.linkswarm.* 与 linkswarm/a3，用于多机器人组网、任务分发与资源同步。

任务消息含 Task、TaskAudio / TaskEmoticon / TaskMotion / TaskMove；轨迹用 TrajectoryPos / TrajectoryVel；心跳经 LinkSwarmHeartbeatChannel 广播。

10.4 系统管理（setting / OTA / 紧急模式）

设置服务（setting/）

状态经 RobotSettingChannel 广播；操作枚举如 WifiControlOperation / WifiSecurity / VolumeControlOperation / BlueToothControlOperation / BlueToothConnectState 等定义于对应 .proto。

OTA 升级（ota/）

AimDK OTA 采用三层架构（Master / Gateway / Slave），分别对应主控端、网关端和各从节点（SOC/MCU/固件/软件）。顶层服务 OTAService 提供通用入口，A3 Ultra 主控使用 OTAMasterService。

标准升级流程（OTAMasterService）

主控级 OTA 共 6 步，按序调用：

接口调用示例

说明：SDK 的 proto 定义中未给出 OTA 服务的端口号与运行节点（HDU/ADU/MDU 归属未在协议中声明）。以下示例中的 IP 与端口需在真机上确认后替换。真机上可通过 ss -tlnp | grep ota 或查看 OTA 进程配置来确认监听端口。

① 状态机查询

curl -X POST 'http://<OTA节点IP>:<OTA端口>/rpc/aimdk.protocol.OTAMasterService/StateMachineQuery' \
     -H 'Content-Type: application/json' \
     -d '{"header": {"control_source": 0}}'

② 查询是否有新版本

curl -X POST 'http://<OTA节点IP>:<OTA端口>/rpc/aimdk.protocol.OTAMasterService/NewTaskQuery' \
     -H 'Content-Type: application/json' \
     -d '{
       "header": {"control_source": 0},
       "language": "zh-CN"
     }'
# 响应：has_new_task / release_id / version / release_notes / type

③ 开始下载 OTA 包

curl -X POST 'http://<OTA节点IP>:<OTA端口>/rpc/aimdk.protocol.OTAMasterService/StartDownload' \
     -H 'Content-Type: application/json' \
     -d '{"header": {"control_source": 0}}'
# 下载进度通过 OTAMasterHeartbeatChannel 心跳或 /ota/schedule 订阅获取
# 可暂停/继续/中止：PauseDownload / ContinueDownload / AbortDownload

④ 开始升级

curl -X POST 'http://<OTA节点IP>:<OTA端口>/rpc/aimdk.protocol.OTAMasterService/StartUpgrade' \
     -H 'Content-Type: application/json' \
     -d '{"header": {"control_source": 0}}'

⑤ 查询升级进度（顶层 OTAService）

curl -X POST 'http://<OTA节点IP>:<OTA端口>/rpc/aimdk.protocol.OTAService/GetCurrentOtaProgress' \
     -H 'Content-Type: application/json' \
     -d '{"header": {"control_source": 0}}'
# 响应：current_states / percentage / error_count / current_apk_version

⑥ 获取升级结果

curl -X POST 'http://<OTA节点IP>:<OTA端口>/rpc/aimdk.protocol.OTAMasterService/GetOTAResult' \
     -H 'Content-Type: application/json' \
     -d '{"header": {"control_source": 0}}'

OTA 话题与广播

OTAStage 从机升级状态枚举

Idle(1) / Upgrading(2) / Upgrading_Success(3) / Rollbacking(4) / Rollbacking_Success(5) / Pause(6) / Failure(7) / PrepareReady(8) / Upgrading_Image(9) / Wait_Active(10) / Finish_Active(11) / Peripheral_Ready(12)

系统 / 紧急模式（hal/state + sm/）

SM 系统状态 SystemStatus：IN_INITIAL / IN_READY / IN_MOVE / IN_ROLLBACK / IN_RECOVERY / IN_FALLBACK / IN_FALLBACK_MOVE。紧急模式下 MC 会 deactivate，恢复后需 ReleaseEStop，再 activate mc + activate motion_player（见 0.5 安全红线与 9.3 急停复位流程）。

12. 接口速查总表

附录

附录 A：SDK 目录结构

agibot_a3_Ultra_aimdk-dev3.2/
├── protocol/                  # 原始协议定义
│   ├── ros2/                  #   ROS2 消息/服务定义(按功能板块)
│   └── protobuf/              #   protobuf .proto 源(按功能板块)
├── examples/                  # 全接口示例
│   ├── agent/                 #   TTS/音频/麦克风/唤醒/焦点
│   ├── hal_audio/             #   音量
│   ├── hds/                   #   告警
│   ├── mc/                    #   运动控制(走/臂/头/手/腰/Action/关节状态)
│   ├── mm/                    #   地图(建图/定位/UI演示)
│   ├── other/                 #   BMS/急停
│   ├── pnc/                   #   规控导航(RPC 调用示例+demo)
│   ├── resource_manager/      #   资源创建/查询/删除/迁移(+示例资源文件)
│   └── skill_play/            #   动作/表情/舞蹈/技能状态
├── prebuilt/                  # 预构建协议文件
│   ├── audio_msgs_proto_aarch64/
│   ├── ros2_plugin_proto_aarch64/   # RosMsgWrapper 环境
│   └── a3_aimdk-3.2.0-py3-none-any.whl   # Python 协议包(PB 消息)
├── README.md / password.md / format.sh / sync_protobuf.py
└── .clang-format / .pycodestyle

附录 B：协议子包（示例）

Python 包顶层为 aimdk.protocol，按能力域分组如下：

基础能力域

• motion_control / mc：action 状态机 / motion 行走 / joint 关节服务（腿/臂/腰）/ move_waist / data(joint 状态) / kinematics / safety / simulation
• mm（地图）：mapping / localization / relocalization / qr_localization / occupancy_grid
• pnc（导航）：pnc_service / path / 规控相关（见第 6 章）
• interaction / agent：tts_service(TTSStatus) / interaction_service / agent_data(WakeUpResult) / asr_result
• hal：audio / bms / camera / hand / neck / power / sensors / state(emergency/operation_mode) / light / thermal / uwb
• resource_manager：资源上传 / 校验 / 列表（见第 7 章）
• skill / aim_master：动作/表情/舞蹈播放、HFAEmoction 皮肤表情（见第 8 章）
• hds：alert 告警 / exception 异常 / health 健康 / monitor 监控（见第 9 章）
• common：header / rpc / timestamp — 通用请求/响应头、错误码 CommonState

高级能力域（第 10 章）

• perception：perception_service / object_2d_detection / object_6d_pose_estimation / target_guidance / tracked_obstacles / object_mask
• manipulation：manipulation_service / manipulation_msg — 机械臂抓取操作
• task_engine：task_engine_service / task_engine_calibration / worker_service / grasp_objects — 任务引擎与标定
• ta（遥操）：ta_service / ta_whole_body_command / ta_whole_body_state / teleop_body_mode
• sm（状态机）：sm_rpc / sm_api / sm_state / sm_channel — 系统状态与急停
• linkswarm：linkswarm_robot / linkswarm_task / linkswarm_resource / linkswarm_a3 — 多机群组
• gateway：auth（InteractionAuthService）/ login（AccessControl）/ log / trace — 鉴权与多端管理
• setting：wifi / volume / bluetooth / robot_info / function_setting / system_setting — 系统设置
• ota：ota_master / ota_slave / ota_channel — 系统升级
• media：tts / asr / webrtc — 媒体通道

附录 C：常见问题（FAQ）