第 7 章 执行与实时接口¶
规划给出的是文件级的东西：一条带时间戳的关节轨迹。让它变成机械臂的实际运动，靠的是另一套进程与接口，而这一层的失败几乎不解释原因——只给一个返回码。本章把执行链的入口、监控参数、故障形态与量纲分工逐一说清，并说明哪些故障会被立刻拒绝、哪些会被静默接受。运行环境 ROS 2 Humble + MoveIt 2 2.5.10 + ros2_control 2.54.3，样本栈为 panda 样本 demo（假硬件）。
轨迹到关节运动的执行链与失败投影：
flowchart LR
traj["规划轨迹"] --> exec["执行入口<br/>execute / controller"]
exec --> mon["执行监控：三个容差"]
mon --> code["返回码：拒绝 vs 静默接受"]1. 三个入口与它们的可见性¶
轨迹进入控制层有三条通路，区别在于失败能不能被调用方看到：
| 入口 | 类型 | 调用方 | 失败可见性 |
|---|---|---|---|
/{controller}/follow_joint_trajectory |
动作（control_msgs/action/FollowJointTrajectory） |
MoveIt 的执行管理、外部轨迹发送器 | 有返回码、feedback、停止原因 |
/{controller}/joint_trajectory |
话题（trajectory_msgs/JointTrajectory） |
Servo、遥操作、外部插补器 | 无反馈，丢了也不知道 |
直接硬件接口（write() 到关节指令） |
无 ROS 语义 | 自定义控制器 | 取决于驱动 |
MoveIt 走第一条（moveit_simple_controller_manager 按 moveit_controllers.yaml 里的 action_ns: follow_joint_trajectory 找动作），Servo 走第二条（配置里 command_out_topic 就指向 panda_arm_controller/joint_trajectory）。二者的差别不是性能而是语义：动作有目标、反馈、结果三段，话题只有单向流；所以位置跟随类的安全判据（超时、容差、卡住）只在动作通路上存在。第 6 章的抓取时序之所以能靠返回码分支处理，是因为它用的是动作通路。
在 demo 里能看到这三个入口并存：
ros2 action list && ros2 topic list | grep joint_trajectory
/execute_trajectory
/move_action
/panda_arm_controller/follow_joint_trajectory
/panda_hand_controller/gripper_cmd
/execute_trajectory（moveit_msgs/action/ExecuteTrajectory）是 MoveIt 对外的执行入口，/move_action 的执行分支在规划成功后自动串上它；夹爪走自己的 gripper_cmd（第 6 章）。
三条入口的差别不在性能，在失败能不能被看到：
flowchart LR
traj["轨迹来源"] --> act["动作入口<br/>follow_joint_trajectory<br/>有反馈 / 结果 / 停止原因"]
traj --> topic["话题入口<br/>joint_trajectory<br/>无反馈，丢了不知道"]
traj --> raw["直接硬件 write()<br/>取决于驱动"]
act --> safe["安全判据只在动作通路<br/>超时 / 容差 / 卡住"]2. 执行监控的三个容差¶
MoveIt 侧的执行监控参数写在配置里，不是硬编码：
## gripper_moveit_controllers.yaml 头部：执行监控的三个容差
trajectory_execution:
allowed_execution_duration_scaling: 1.2 # 实际耗时 ÷ 计划时长 的上限
allowed_goal_duration_margin: 0.5 # 额外宽限的绝对时长 [s]
allowed_start_tolerance: 0.01 # 轨迹起点与当前状态的容差（弧度或米）
三条各自防不同的事。allowed_start_tolerance 防“起点不符”：控制器接手一条轨迹时，第一个航点必须落在当前状态的容差内，否则轨迹与实机不连续。allowed_execution_duration_scaling 与 allowed_goal_duration_margin 防“跑不完”：实际耗时超过 1.2 × 计划时长 + 0.5 s 即判失败。
起点容差这一条在跨层协作里最容易踩：规划用的当前状态来自 /joint_states（joint_state_broadcaster 与 zero_joint_states 两个发布者共同贡献，/joint_states 上没有对应关节时该关节在规划里就是“未知”），而执行侧的当前值来自状态接口。两侧一旦不同步（传感未发布、话题丢失、仿真与规划分离），第一条轨迹就会被拒。
3. 故障在返回码上的投影¶
执行链对五类输入的处理如下（轨迹为 2 s、7 关节、含速度与时间戳；mock 硬件）：
| 注入 | 结果码 | 码名目 | 说明 |
|---|---|---|---|
| 正常轨迹 | 1 | SUCCESS |
计划 2.0 s，执行成功 |
| 起点与当前状态差 0.05 rad | −4 | CONTROL_FAILED |
超过 0.01 的起点容差 |
| 关节名顺序打乱 | −4 | CONTROL_FAILED |
控制器按名分发，序列不一致即拒绝 |
| 中间点越过 joint4 上限（给 0.5，上限 0.0873） | −4 | CONTROL_FAILED |
越限点在控制器侧被拒 |
| 8 s 轨迹执行 1 s 后取消 | −4 | CONTROL_FAILED |
取消与失败共用同一码 |
四条要防的结论。
返回码是一个筐。 起点不符、名字不匹配、越限、主动取消，全都返回同一个 CONTROL_FAILED（−4）。仅凭它无法归因，必须同时看控制器侧的输出（JTC 会说明拒绝原因）与下发时的具体条件。工程上可用的做法是给每条下发记一条本地断言（起点误差、关节名集合、每点是否越限），把归因留在调用方，而不是指望返回码。
取消不是独立状态。 期望“取消”与“失败”能区分时，只能自己记账：发出取消请求的时刻与收到结果的时刻关系、以及末端实际位置与轨迹终点的距离，才构成“被取消”的判据。
关节名顺序是显式契约。 打乱名字序列被立刻拒绝，说明执行层按名而不是按位分发；这也意味着任何“按索引填位置”的代码在改名或换组时会静默错位到别的关节——这是被执行层拒绝的幸运案例，落到话题通路（第 1 节表中的第二条，无反馈）就成了无声事故。下发前核对 joint_names 与顺序是必做检查。
控制器会拒绝越限点，但规划侧的限位才是主防线。 越限点在执行侧被拒，说明模型限位与控制器配置是两道独立的检查；只靠后者兜底会把大量失败推到运行时（第 1 章的 joint_limits.yaml 覆写属于第一道）。
4. mock 层与真机的差异清单¶
同一台“机器”在有无物理的两层里表现不同。以 2 s 的三段航点轨迹为例：计划时长 2.0 s，执行成功，但按轨迹线性插值采样得到的“实际位置”与计划剖面最大可差一整个航点间距（样本轨迹里 11.45°）。这个数字不是控制残差——mock_components 不插值跟随，它把状态直接设成当前目标航点，于是航点之间的时刻上“实际位置”与计划的线性插值剖面可以差到一整个航点间距。正确读法是：mock 层的到位误差是零，段内误差是航点间距。
这条区别决定了哪些结论能在这层写：
| 能验 | 验不了 |
|---|---|
| 接口时序、动作目标与反馈的语义、容差检查是否触发 | 跟踪误差、扰动下的稳定性 |
| 轨迹形状合法性能被执行层接受（起点、名字、限位） | 接触、滑移、工件变形 |
| 段数、时长、航点密度与规划参数的对应关系 | 实际节拍（无动力学与时延） |
| 取消、抢占与错误码分支 | 力/力矩相关的一切（第 8 章） |
所以第 6 章抓取闭环的“到位误差 3–4 mm”是链路与几何层的数字，不含控制残差；含控制的结论必须在物理层（ros_gz）或真机重做（第 6 章末）。
反馈侧还有一处要记：/joint_states 有两个发布者（joint_state_broadcaster 与 zero_joint_states），话题频率读数约 120 Hz，高于 controller_manager 的 update_rate: 100。两个来源的关节子集不同、时间戳各自生成，把话题频率当控制器周期、或把某一条消息当成完整状态快照，都会得出错误的时延结论。要精确判断某关节的更新率，按发布者分别测。
5. 量纲分工¶
链路上四个时间尺度互不重叠，任何一层的参数拿到另一层都用不上：
| 环节 | 周期/时长量级 | 参考值 |
|---|---|---|
| 规划决策 | 10⁻³ ~ 10⁻¹ s 一次请求 | 第 4 章 0.001–0.013 s 出解 |
| 伺服指令发布 | 约 30 Hz | Servo 配置 publish_period: 0.034 s |
| 轨迹跟随 | 100 Hz | controller_manager 的 update_rate: 100 |
| 关节伺服内环 | 千赫兹级 | 驱动器内部，本栈不可见 |
| 一条轨迹的时长 | 秒级 | 第 1 章 1.779 s、第 6 章分段 1–3 s |
采样规划的一次解要经过重定时（第 4 章）、下发、100 Hz 跟随、驱动器内环，才落到关节上；这就是“调规划器参数去消除跟踪抖动”无效的算术根据。规划侧能改的是轨迹的形状与时间预算，控制侧决定的是跟随质量。控制环侧的整定与相位分析归 control-note 第 1、5 章。
6. Servo 的接口边界与集成坑¶
MoveIt Servo（moveit-servo 2.5.10）提供的是实时伺服执行：吃笛卡尔速度或关节速度指令，输出连续轨迹给控制层，用于遥操作与视觉伺服（第 9 章的回路也走它）。它与轨迹执行的分工是：轨迹执行是“给定一条已经存在的轨迹，执行完”；伺服是“给定一个速度意图，持续产生指令”。因此前者有终点与结果码，后者只有状态与超时保护。
panda 样本配置给出的参数形态可以直接读出来：输入 delta_twist_cmds（笛卡尔速度）与 delta_joint_cmds（关节速度），planning_frame: panda_link0，指令量纲 command_in_type: "unitless"（[-1,1] 归一化，经 scale.linear: 0.4 / scale.rotational: 0.8 / scale.joint: 0.5 换算成速度），输出 command_out_topic: /panda_arm_controller/joint_trajectory（即第 1 节表中的话题通路，不经过动作）。
集成时要同时满足三组条件：参数文件的形态、节点名字与命名空间的解析、以及输入链的前置条件。
一、发行版参数文件不能直接喂给 ROS 2 的解析器。 moveit_servo/config/panda_simulated_config.yaml 用的是扁平写法（顶层直接给 use_gazebo 等键），而 ROS 2 的参数文件要求 <node>: ros__parameters: 结构，直接用会让节点在解析参数阶段就退出。需要把整份内容缩进、包进 ros__parameters 下再加载。
二、命名空间与私有名字叠加。 用 namespace="servo_node" 启动时，节点内部以 ~ 声明的话题会变成 /servo_node/servo_node/delta_twist_cmds 这类双段路径：命令、状态话题实际落在叠加后的名字上，按直觉写 /servo_node/delta_twist_cmds 会静默发不到（话题存在但没有订阅者）。集成时先 ros2 topic list 核对实际路径，再决定是设命名空间还是设节点名。
三、输入链的四个前置条件。 Servo 的输出话题必须落到接受该消息类型的控制器上（trajectory_msgs/JointTrajectory → JTC 的话题入口，而不是只接动作的通路）；moveit_active 由 Servo 的规划场景监控置位，场景未就绪时指令不会被执行；指令看门狗与 incoming_command_timeout 决定“多久没有新指令就停”；current_pose 需要 TF 在 planning_frame 下持续提供。这四项任一不满足，表现都是命令话题有流量而关节不动，排查顺序按此逐项核对。伺服的吞吐量、延迟等性能数字取决于机器与网络，接口层面不给出通用值。
7. 失败的恢复与状态一致性¶
执行失败的恢复比失败判定更难，因为机器人此刻在哪决定了下一步能不能做。可复用的处置顺序：
- 停止下发新的轨迹或伺服指令（伺服侧靠指令超时自动停止：不再有输入即减速停下，这也是它比话题直发安全的原因）。
- 读取实际状态（
/joint_states+ TF），而不是信任“计划已执行到第几步”。第 3 节的取消实验说明：动作返回码不带进度信息。 - 把场景恢复到已知一致的状态：附着体、临时物体、被半途松开工件（第 6 章的
detach语义）都要重新核对，否则下一次规划会踩在第 3 章那个“残留附着体毒化所有规划”的坑上。 - 用一次短距离、低缩放（第 4 章的缩放因子）的规划验证链路恢复，再回到原任务。
这条顺序里最容易被跳过的是第 3 步。它同时是第 10 章排障清单的第一项：规划突然全线失败，先查场景残留。
8. 要点¶
- 轨迹进控制层有动作与话题两条通路：动作有目标、反馈、结果码，话题只有单向流；需要判失败就要用动作通路。
- 执行监控的三个容差是配置项：起点容差 0.01、耗时缩放 1.2、宽限 0.5 s；起点容差的失败来自规划侧状态与执行侧状态不同步。
- 起点偏差、关节名乱序、越限点、主动取消都落到同一个
CONTROL_FAILED（−4）：归因必须留在调用方（下发前断言 + 控制器日志），不能指望返回码。 - mock 层到位误差为零、段内误差等于航点间距（样本轨迹里最大 11.45°）；
/joint_states由两个发布者合成、约 120 Hz，不等于控制器周期。 - 链路上五个时间尺度互不重叠（规划毫秒、伺服约 30 Hz、跟随 100 Hz、内环千赫兹、轨迹秒级），跨层调参是分层误用。
- Servo 的输入是速度意图、输出走话题通路；两条集成坑（参数文件需
ros__parameters包裹、命名空间叠加导致话题路径变化）之外，还要核对输出话题的接收方、moveit_active、看门狗与current_pose四个前置条件；性能数字取决于机器与网络，接口层面不给通用值。 - 失败恢复的顺序：停止下发 → 读实际状态 → 清场景残留 → 低缩放短程验证；被跳过最多的就是清残留。