ROS 2 Action 取消语义：收到 cancel 不等于执行器已经停下

操作员点了取消，Action 客户端很快收到 CANCEL_GOAL 成功，机械臂却又走了半秒——这类事故通常被归咎于「驱动慢」，根因更常见：把 cancel 被接受当成了执行器已停止。在 ROS 2 里，取消是状态机上的请求与协商，不是瞬时全局刹车。

主线：分清 client、server、执行线程、硬件四条时间线，把「可取消」写成可观测的终态契约；再落到 rclcpp_action API、执行器与 ros2_control 抢占、多线程 executor 的真实交互。

1. 完整状态机：从 ACCEPT 到终态

rclcpp_action / rclpy 的 goal handle 不是三态开关。按 ROS 2 Action 协议与实现习惯，一条目标大致经历：

pending ──(goal_callback ACCEPT)──► accepted
                                      │
                                      ▼
                                  executing
                                 /         \
                    cancel ACCEPT           正常完成 / 失败
                           │                    │
                           ▼                    ▼
                       canceling          succeeded / aborted
                           │
              settled OK / timeout
                     │           │
                     ▼           ▼
                 canceled      aborted

终端态只有三类：succeeded、canceled、aborted。客户端 async_cancel_goal 返回 CancelResponse::ACCEPT（或 Python 侧等价成功），只说明服务端接受了取消请求并进入 canceling 相关路径，不保证：

• 执行回调已经看到取消标志；
• 底层控制器已经收到零速度或 hold；
• 最后一条 feedback / result 已经反映物理静止。

服务端在 goal_callback 接受目标后，真正工作多在独立执行线程、std::async、或与 executor 解耦的工作队列里。取消请求由 cancel_callback 决策：ACCEPT 或 REJECT。ACCEPT 之后，执行循环仍需主动轮询 handle->is_canceling()（或 rclpy 的 GoalHandle.is_cancel_requested）并停止业务。

把「cancel accepted」映射成 HMI 上的「已停止」，是接口层语义错误：中间件只保证协议推进，不保证动力学收敛。验收字段里应同时出现协议时间戳与测量域速度，缺一就视为契约未完成。

1.1 rclcpp_action 关键回调与句柄 API

服务端创建时挂三组回调：

using GoalHandle = rclcpp_action::ServerGoalHandle<MyAction>;

auto server = rclcpp_action::create_server<MyAction>(
  node,
  "move_arm",
  // goal_callback: 是否接受新目标
  [this](const rclcpp_action::GoalUUID &, std::shared_ptr<const MyAction::Goal> goal) {
    if (!policy_.allows(*goal)) {
      return rclcpp_action::GoalResponse::REJECT;
    }
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  },
  // cancel_callback: 是否接受取消（短、非阻塞）
  [this](const std::shared_ptr<GoalHandle> handle) {
    if (!policy_.cancelable(handle)) {
      return rclcpp_action::CancelResponse::REJECT;
    }
    return rclcpp_action::CancelResponse::ACCEPT;
  },
  // accepted_callback: 启动执行体
  [this](const std::shared_ptr<GoalHandle> handle) {
    // 切勿在此阻塞做轨迹；交给执行线程
    workers_.post([this, handle] { execute(handle); });
  });

执行体内常用句柄方法：

CancelResponse::REJECT 的合法场景包括：目标已进入终端态、策略禁止中途取消、安全联锁要求必须走急停话题而非软取消。拒取消时客户端必须展示真实状态，而不是自旋重试到硬件抖动。

2. 四条时间线与三条异步边界

把一次取消画成四条并行时间线：

Client:   cancel req ──► cancel resp ──► wait result ──► HMI
Server:        cancel_cb ──► flag ──► execute loop ──► result
Control:                    hold/clear ──► controller ACK
Plant:                              braking ──► |v|<ε

Client cancel ──► Action server cancel_callback ──► 执行循环采样
                                              └──► 控制器/驱动停止
                                              └──► result(canceled)

边界 A：cancel_callback 与执行循环。
cancel_callback 必须短：只做策略判断与置位。它不应阻塞式调用驱动停止；否则会卡住负责分发 Action 回调的 executor 线程，反而拖慢真正的停止路径。执行循环必须以足够高的频率采样取消标志——采样周期是「协议取消」到「命令域停止」的下界之一。

边界 B：执行循环与控制器。
即使循环立刻 break，若向 ros2_control 或厂商驱动写入的是「轨迹队列」，还需要显式 hold / clear / deactivate。只停止写入新 setpoint，旧队列仍可能继续消耗。对 JointTrajectoryController 一类实现，常见手段是发空轨迹、抢占为 hold 点，或切换控制器；具体取决于你绑的是位置接口还是速度接口。

边界 C：result 与物理世界。
canceled 结果应在「命令域停止」达成后发布，还是在「测量域静止」后发布？这必须在接口文档写死。对夹爪、升降机等有惯性的机构，两者差一个制动时间 $T_{\text{brake}}$。若契约写「测量域静止」，则：

且在 $t_{\text{result}}$ 之前必须满足 $|v|<{\epsilon }_v$（或等效关节误差阈值）。超时则走 abort，不要粉饰成 canceled。

3. 可中断执行循环的最小形状

下面以 rclcpp_action 风格示意（简化，突出采样点与终态分流）：

void execute(const std::shared_ptr<GoalHandle> handle) {
  const auto goal = handle->get_goal();
  auto feedback = std::make_shared<ActionT::Feedback>();
  auto result = std::make_shared<ActionT::Result>();

  rclcpp::Rate rate(control_hz_);
  while (rclcpp::ok() && handle->is_active()) {
    if (handle->is_canceling()) {
      command_iface_->send_hold();          // 命令域停止
      if (!wait_motion_settled(deadline_)) {
        result->error_code = ErrorCode::CANCEL_TIMEOUT;
        handle->abort(result);              // 未能按契约停下
        return;
      }
      result->error_code = ErrorCode::OK;
      handle->canceled(result);
      return;
    }
    if (deadline_exceeded()) {
      command_iface_->send_hold();
      result->error_code = ErrorCode::GOAL_TIMEOUT;
      handle->abort(result);
      return;
    }
    command_iface_->send_setpoint(next_setpoint(goal));
    feedback->progress = progress();
    handle->publish_feedback(feedback);
    rate.sleep();
  }
}

几个刻意设计：

• 取消分支先发 hold，再等 settled，再 canceled；
• 超时与取消分流：超时用 abort，避免把「没停住」粉饰成「已取消」；
• feedback 仍可在 canceling 期间发布，便于 HMI 显示制动进度；
• is_active() 防止对已终态句柄继续写命令。

若执行体是一次性 async 轨迹下发、没有循环，就在 cancel 路径注册「轨迹抢占」回调，并同样等待抢占完成事件；没有完成事件就不要宣称 canceled。

3.1 实时线程与 executor 的职责切分

现场常见两种部署：

1. 非实时节点 + 实时控制器。 Action server 跑在普通 MultiThreadedExecutor；真正的 $1,\mathrm{kHz}$ 电流环 / 位置环在 ros2_control 的 realtime 线程。取消路径应尽快把「停止意图」变成控制器可消费的命令（hold 点、空轨迹、bool 急停接口），而不是在 Action 线程里做忙等自旋到毫秒级控制周期。
2. Action 与控制同进程。 仍要把「协议回调」和「硬实时循环」隔开：cancel_callback / accepted_callback 只投递无锁或有界队列消息；实时循环每周期读标志。跨线程共享的是「下一拍命令」与「状态快照」，不是在回调里直接碰实时缓冲。

经验规则：任何可能阻塞超过一个控制周期的工作，都不属于 cancel_callback。 wait_motion_settled 属于执行线程或专用监视线程；若它与 executor 抢同一互斥回调组，会把整个节点拖死。实时优先权倒置也会伪装成「取消慢」：普通优先级的 Action 线程拿着锁不放，高优先级控制线程反而等锁——用优先级继承或把锁粒度限制在非实时侧的命令槽上。

4. ros2_control 抢占与轨迹队列

Action 取消落到运动栈时，真正要回答的是：控制器当前的 setpoint 源会被什么替换？

对 joint_trajectory_controller：

• 新轨迹消息通常抢占旧轨迹；取消若只停 Action 循环、不发替代轨迹，控制器可能继续执行队列尾部。
• 软停常用「当前状态为起点、时长极短的 hold 轨迹」，或控制器提供的 stop/hold 接口。
• 硬停走安全层（安全 PLC、/emergency_stop、力矩限幅），不要指望 Action canceled 代替安全功能。

统一内部的 preempt_motion(reason) 很有价值：

enum class PreemptReason { kCancel, kNewGoal, kWatchdog, kEstop };

void preempt_motion(PreemptReason reason) {
  command_iface_->send_hold();
  command_iface_->clear_queue();   // 若接口支持
  diagnostics_.set(reason);
}

人手取消、新 goal 抢占、看门狗超时，三条入口共用同一停止实现，避免「取消走慢路径、抢占走快路径」导致停止质量不一致。

4.1 与新 goal 的时序契约

Action 服务常配置为接受新 goal 时抢占旧 goal。抢占与取消都通向「旧目标不再执行」，但结果语义不同：被新目标替换时，旧 handle 可能走向 aborted 或 canceled，取决于你在 server 内如何收口。建议写死：

1. 旧 goal：先 preempt_motion，再按策略发 canceled 或 aborted（文档二选一，全栈一致）；
2. 新 goal：仅在「命令域已切换到新目标或 hold」之后开始输出运动；
3. 若允许短重叠，必须写明重叠窗口内的最大速度/加速度，并在验收里测。

否则集成方会在「取消按钮」与「连续下发新目标」两条操作路径上看到不同的制动距离。

5. 多线程 Executor 与回调组

取消语义的 bug 有一半其实是调度问题。

单线程 SingleThreadedExecutor：
cancel_callback、feedback 发布、其它定时器共享一条队列。若 execute() 误跑在同一线程且长时间 sleep，取消回调会被饿死——客户端已发出 cancel，服务端标志却迟迟不置位。正确做法是执行体进工作线程，或把长循环拆成定时器状态机，每次回调很快返回。

MultiThreadedExecutor：
能并行跑不同回调组里的回调，但默认 MutuallyExclusiveCallbackGroup 仍会串行化组内工作。典型坑：

• 把 Action server 回调与阻塞式 wait_motion_settled 放进同一互斥组 → cancel 后监视逻辑堵住后续 goal；
• 在执行线程里再 spin_some / 嵌套 spin → 难预测的重入；
• 多客户端并发 cancel / 新 goal 时，对共享 command_iface_ 无互斥 → 双 hold、双 clear 交错。

推荐拆分：

客户端侧若在回调里同步 spin_until_future_complete 等 result，同样会嵌套 spin；应 async_cancel_goal + result future 的 done callback，或在独立线程等待并设总截止时间。

6. 客户端侧：不要只等 cancel 响应

客户端正确序列通常是：

1. async_cancel_goal；
2. 等待 cancel response（是否被接受）；
3. 继续等待 result，并设置总截止时间 $T_{\text{client}}$；
4. 截止时间到达仍无终端态 → 进入故障响应（急停话题、切控制器、报警）。

auto cancel_future = client->async_cancel_goal(goal_handle);
if (cancel_future.wait_for(cancel_resp_timeout) != std::future_status::ready) {
  // 连「是否接受取消」都未知 → 升级安全路径
  estop_->trigger("cancel_response_timeout");
  return;
}
auto result_future = client->async_get_result(goal_handle);
if (result_future.wait_for(total_timeout) != std::future_status::ready) {
  estop_->trigger("cancel_result_timeout");
  return;
}
// 仅当 result.code == CANCELED 且业务 error_code==OK 时，HMI 才显示「已按契约停止」

只听 cancel response 就点亮绿灯，等于忽略边界 B/C。对多客户端场景，还要处理「取消被拒」：例如已经 succeeded，或 server 策略不允许中途取消。

7. 截止时间、看门狗与诊断

取消路径必须有自己的 deadline。否则执行线程卡在「等 settled」会让 server 对后续 goal 失聪。分层建议：

• 命令确认时限 $T_{\text{ack}}$：hold 指令被控制器 ACK 的上限；
• 运动收敛时限 $T_{\text{settle}}$：速度/力矩跌破阈值的上限；
• result 发布时限 $T_{\text{result}}$：相对 cancel 请求的总上限，满足 $T_{\text{result}}\ge T_{\text{ack}}+T_{\text{settle}}$。

任一层超时都应变为 abort + 诊断码，并触发更上层的安全响应。diagnostic_updater 或自定义 /emergency_stop 比在日志里打 warning 更有用。把四段时间戳打进同一结构化日志：

现场说「取消慢」，先看慢在哪一段：协议、中间件、控制接口，还是机构制动。

8. Launch / 集成测试：把契约钉死

用 launch_testing 或硬件在环脚本覆盖下列场景（示意结构）：

# test_cancel_semantics.py（launch_testing 风格节选）
@pytest.mark.launch_test
def generate_test_description():
    server = Node(
        package="arm_bringup",
        executable="move_action_server",
        parameters=[{"control_hz": 200, "settle_timeout_ms": 300}],
    )
    fake_controller = Node(package="arm_bringup", executable="fake_controller")
    return LaunchDescription([
        server,
        fake_controller,
        launch_testing.actions.ReadyToTest(),
    ])


class TestCancelSemantics(unittest.TestCase):
    def test_cancel_during_motion(self, proc_output):
        # 1) 发 goal  2) 运动中 cancel  3) 断言 result=CANCELED
        # 4) 断言 cancel 后 T ms 内 |v| < eps（读 /joint_states 或 fake 状态）
        ...

    def test_controller_silence_aborts(self, proc_output):
        # 砍掉状态话题：应 abort(CANCEL_TIMEOUT)，不得永久 canceling
        ...

验收矩阵：

1. 执行中取消：result 为 canceled，且取消后 $T$ 毫秒内速度低于阈值；
2. 并发双取消：两个客户端同时 cancel，server 不崩溃，终端态唯一；
3. 取消后失联：砍掉 controller 状态话题，server 应超时 abort，而不是永远 canceling；
4. 取消与新 goal 交织：旧 goal 终态先于新 goal 的运动输出，或文档允许的明确重叠策略；
5. 拒绝路径：在终端态后再 cancel，客户端收到 reject，HMI 不显示「已停止成功」；
6. executor 压力：并行灌其它定时器/服务时，cancel 的 ${\Delta }_{\text{proto}}$ 仍低于预算。

可观测信号建议同时记录：cancel 请求时间戳、cancel response 时间戳、hold 下发时间戳、速度过阈时间戳、result 时间戳。四段延迟分开看，才能知道慢在状态机、中间件还是驱动。

9. 文档里应写清的三句话

给集成方的接口说明至少包含：

1. 取消被接受的含义（状态机），不包含的含义（物理静止）；
2. canceled 结果发布前必须满足的测量条件（命令域 / 测量域二选一写死）；
3. 超时与 abort 的升级路径（含急停话题或控制器切换）。

补充两行能减少半年扯皮：新 goal 抢占时旧 goal 的终态码；多线程部署下「取消采样频率」与 control_hz 的关系。

10. cancel_callback 与执行体的线程契约（再落一层）

协议层把取消做成「请求—应答」，执行体却往往跑在别的线程。若二者共享的只有一个 std::atomic_bool canceling，却没有对命令接口的互斥，现场会出现：执行线程刚写出下一拍速度，取消线程紧接着 send_hold()，随后执行线程又用旧的 next_setpoint() 覆盖 hold。协议上已经 canceling，命令域却在抖动。

最小契约可以写成：

struct MotionSlot {
  std::mutex mu;
  enum class Mode { kRun, kHold, kClear } mode{Mode::kRun};
  TrajectoryPoint setpoint{};
};

void execute(...) {
  while (...) {
    if (handle->is_canceling()) {
      {
        std::lock_guard lock(slot_.mu);
        slot_.mode = MotionSlot::Mode::kHold;
        slot_.setpoint = current_measured_as_hold();
      }
      // 等待监视线程/控制桥确认 ACK 与 settled
      ...
    }
    auto sp = next_setpoint(goal);
    std::lock_guard lock(slot_.mu);
    if (slot_.mode != MotionSlot::Mode::kRun) {
      continue;  // 已被抢占，禁止再写 Run 设定
    }
    slot_.setpoint = sp;
  }
}

控制桥（或 ros2_control 侧的非实时写入线程）只读 MotionSlot：一旦 mode!=kRun，只下发 hold/clear，不再消费执行体的 Run 设定。这样「取消采样」与「命令仲裁」分离——采样可以在 Action 线程，仲裁必须对所有写入者可见。

若用无锁 SPSC：执行体是唯一生产者，取消路径只许写 mode 标志（原子），由执行体自己在下一圈把 setpoint 改成 hold。两种模型都行，忌讳的是「取消路径与执行体同时写硬件接口」。

10.1 反馈在 canceling 期间发什么

publish_feedback 在 canceling 阶段仍然合法，而且往往比 result 更早到达 HMI。建议反馈字段至少区分：

• phase: executing / canceling / settling；
• speed_norm 或关节最大 $|\dot{q}|$；
• hold_acked: 控制器是否已确认 hold；
• eta_ms: 按当前减速度估计的静止时间（粗即可）。

HMI 应绑定 phase 与 result，而不是绑定 cancel response。操作员看到「制动中 0.3s」比看到「取消成功」后半秒仍在动，更不容易误判。

11. 与 Service、Topic 急停的分工

Action 取消是协作式、可拒绝、可携带业务结果的停止；Topic 急停是广播式、应尽快生效、通常不可协商的停止。把两者焊成一条路径，会出现：

• 急停也走 Action cancel → 依赖执行循环采样，实时性差；
• 普通取消直接拉急停 → 机构每次点取消都按故障停机，恢复成本高。

推荐分层：

1. 软取消：Action cancel → hold → settled → canceled；
2. 看门狗失败：Action abort + 诊断，必要时升软急停；
3. 硬急停：独立 Topic / GPIO / 安全继电器，绕过 Action 状态机。

接口文档写清：软取消的 $T_{\text{settle}}$ 预算是多少；超过预算谁有权升硬急停。验收时分别注入，不要用急停去「补」没写完的取消契约。

12. 多 Goal、多客户端的竞态

同一 Action server 上常见竞态：

1. 客户端 A cancel，客户端 B 同时发新 goal。
   goal_callback 与 cancel_callback 可能交错。必须在 server 内对「当前活跃 goal UUID」加锁：要么先完成旧 goal 的 preempt 再 ACCEPT 新 goal，要么 REJECT 新 goal 直到旧 goal 终态。两种策略都可以，但不能默认「回调顺序碰巧正确」。

2. 双客户端对同一 UUID 重复 cancel。
   第二次应得到确定性结果：仍是 ACCEPT（幂等）或 REJECT（已终态）。不要第二次走进又一次 send_hold 把监视状态机重置，导致 $T_{\text{settle}}$ 计时器被刷新到永远等不完。

3. async_cancel_all_goals。
   客户端图省事一把取消全部时，server 要对每个 handle 走同一套 preempt，并保证每个 UUID 恰有一个终态。测试里对「全部取消」断言终态数量等于当时活跃 goal 数。

std::mutex goals_mu_;
std::shared_ptr<GoalHandle> active_;

rclcpp_action::GoalResponse goal_callback(...) {
  std::lock_guard lock(goals_mu_);
  if (active_ && active_->is_active()) {
    // 策略 A：抢占旧目标
    preempt_motion(PreemptReason::kNewGoal);
    finish_preempted(active_);  // canceled 或 aborted，文档写死
  }
  return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

finish_preempted 必须在持锁或明确的状态机外发终态，避免执行线程与 goal_callback 双双调用 canceled()。

13. Launch 测试补强：时间戳与假控制器

仅断言 result.code == CANCELED 不够。假控制器应暴露可注入故障：

class FakeController(Node):
    def __init__(self):
        super().__init__("fake_controller")
        self.declare_parameter("drop_state", False)
        self.declare_parameter("hold_delay_ms", 0)
        self.pub = self.create_publisher(JointState, "/joint_states", 10)
        self.sub = self.create_subscription(
            JointTrajectory, "/joint_trajectory", self.on_traj, 10)
        self.timer = self.create_timer(0.01, self.tick)
        self.vel = 0.5
        self.holding = False

    def on_traj(self, msg: JointTrajectory):
        delay = self.get_parameter("hold_delay_ms").value
        # 短轨迹 / 空点 = hold 请求
        if len(msg.points) <= 1:
            self.get_clock().sleep_for(Duration(nanoseconds=delay * 1_000_000))
            self.holding = True
        else:
            self.holding = False
            self.vel = 0.5

    def tick(self):
        if self.get_parameter("drop_state").value:
            return
        if self.holding:
            self.vel *= 0.8  # 简单制动模型
        msg = JointState()
        msg.velocity = [self.vel]
        self.pub.publish(msg)

测试断言示例：

• hold_delay_ms=0 时，${\Delta }_{\text{settle}}$ 低于预算且 result 为 canceled；
• hold_delay_ms 大于 server 的 settle_timeout 时，result 为 aborted 且 error_code 为 CANCEL_TIMEOUT；
• drop_state=True 时不得永久停在 canceling；
• 记录 /rosout 或自定义 /cancel_trace 中的四段时间戳，CI 对 ${\Delta }_{\text{proto}}$ 设回归上限，防止有人又把重活塞回 cancel_callback。

对真实硬件在环，用同一套断言，只把假控制器换成读真实 /joint_states；预算放宽，但终态分流（canceled vs abort）不变。

14. 参数化契约：把阈值暴露成可回归的旋钮

取消语义若只写在注释里，换一台减速比不同的手臂就会失效。建议把关键阈值做成参数，并在 launch 测试里覆盖边界：

move_action_server:
  ros__parameters:
    control_hz: 200.0
    cancel_ack_timeout_ms: 50
    cancel_settle_timeout_ms: 300
    cancel_result_timeout_ms: 400
    settle_velocity_eps: 0.02
    allow_cancel_after_progress: 0.0   # 0=全程可取消
    preempt_old_goal_result: "canceled"  # 或 "aborted"
    feedback_while_canceling: true

取值原则：

• $T_{\text{ack}}<T_{\text{settle}}<T_{\text{result}}$，且客户端总超时略大于 $T_{\text{result}}$；
• settle_velocity_eps 来自空载制动实验的分位数，而不是编码器噪声地板；
• preempt_old_goal_result 全栈统一，导航、示教、力控节点不要各写各的。

参数变更应触发同一套 cancel 时间戳回归：若有人把 cancel_settle_timeout_ms 从 300 改到 3000 来「消灭 abort」，CI 应能从 ${\Delta }_{\text{settle}}$ 分布看出来——那是在用超时掩藏驱动问题。

15. rclpy 侧对称注意点

Python 服务端常见写法是 asyncio 或线程里跑执行体。与 C++ 相同的坑依然在：

• cancel_callback 里做 time.sleep 等待电机 → 卡住 executor；
• 在 MultiThreadedExecutor 下对同一 GoalHandle 并发调用 canceled() / abort()；
• 用 future.result(timeout) 在回调里死等，导致与 C++ spin_until_future_complete 同类的嵌套等待。

rclpy.action.server.ServerGoalHandle 的 is_cancel_requested 对应 C++ 的 is_canceling()；终态方法同样是 succeed / canceled / abort。客户端序列不变：cancel_goal_async 之后仍要等 get_result_async。语言绑定不同，不改变「协议停 ≠ 机构停」这条主线。团队若双语言并存，用同一份 cancel 时间戳回归用例分别跑 C++ 与 Python server，防止只在一侧修了采样频率。

混用 C++ 控制节点与 Python 任务节点时，取消契约以命令域接口为准（hold 话题、控制器服务），不要假设两端 Action 状态机字节级同步。跨进程只共享：goal UUID、终态码、以及你们约定的 feedback phase。

16. 现场排障顺序

接到「取消了还在动」工单时，按时间戳倒推，而不是先改驱动增益：

1. 客户端是否只等了 cancel response，没等 result？
2. server 日志里 cancel_callback 到执行体采样的间隔是否远大于 1/\textit{control_hz}？
3. hold / 空轨迹是否真正发出（用 ros2 topic echo 或 controller 日志）？
4. 控制器是否仍在消费旧队列？
5. 测量域速度过阈时间是否超过文档预算？
6. result 是 canceled 还是其实已经 abort 却被 HMI 画成成功？

前两问是集成错误，中间两问是控制接口错误，后两问是契约与展示错误。大多数「驱动太慢」最后落在 3 或 4，而不是电机本身。

17. 控制器切换与命令接口模式

有些栈在运动中途把控制器从 joint_trajectory_controller 切到 forward_command_controller 或力控。取消若只清轨迹、不同步控制器状态，会出现「Action 已 canceled、仍在旧控制器的最后一个 setpoint 上积分」。

取消 / 抢占路径应显式包含：

1. 写 hold 到当前激活的命令接口；
2. 若需要切控制器，先确认目标控制器已 active，再允许新 goal 输出；
3. 在控制器切换窗口内拒绝新的业务 goal，或文档允许的话只接受「零运动」goal。

controller_manager 的切换服务本身有耗时；把它算进 $T_{\text{ack}}$ 预算。测试里注入「切换失败」：server 应 abort 并升安全路径，而不是报告 canceled。

与硬件接口的 command_interface 模式（位置 / 速度 / 力矩）也要匹配：速度接口上的 hold 是零速度；位置接口上的 hold 是锁当前位；力矩接口上的「软取消」往往要落到阻抗或重力补偿模式，而不是简单发零力矩。接口文档按模式分别写 settled 条件，避免一个 settle_velocity_eps 打天下。把模式写进 Result 或诊断，能避免上层在速度控制阶段误用位置静止判据，从而提前误报 canceled。

18. 把契约写进 Result 字段

除了 Action 标准终态码，业务 Result 建议带机器可读字段：

# 示意，实际用 .action 定义
int32 error_code          # OK / CANCEL_TIMEOUT / PREEMPTED / ...
string error_msg
builtin_interfaces/Time cancel_request_stamp
builtin_interfaces/Time hold_sent_stamp
builtin_interfaces/Time settled_stamp
float32 peak_speed_after_cancel

这样 HMI、日志管道、事后回放可以不靠猜：canceled 且 error_code=OK 才是「按契约停下」；aborted + CANCEL_TIMEOUT 是「协议取消了但机构没在预算内停下」。很多集成事故来自把所有非 succeeded 都显示成绿色的「已处理」。

19. 收束：协议停与机构停要焊开再焊回

ROS 2 Action 把长任务建模得比 service 更合适，但取消是协议，不是物理定律。把「accepted cancel」和「执行器已停下」焊开，再用可中断循环、与 ros2_control 统一的抢占、分层截止时间和可注入的 launch 测试，把它们重新连成可验收契约——现场才敢把取消按钮交给人。

可执行的落地顺序通常是：先画四条时间线与三段延迟预算；再保证 cancel_callback 只做决策、执行体高频采样；然后把 hold/clear 与新 goal 抢占收成同一 preempt_motion；最后用假控制器注入延迟与失联，把 canceled / abort 分流和四段时间戳钉进 CI。客户端与 HMI 只信任 result 与业务 error_code，不信任 cancel response 本身。

下次在底盘或手臂上看到「取消了还在爬」，先问四件事：cancel_callback 有没有做重活？执行循环采了几次取消标志？控制器队列清了没有？canceled 发在 settled 前还是后？这四个答案比再加一个重试按钮有用得多。若四问都干净而机构仍越预算，那才是驱动或安全层的问题——此时应升硬急停路径，而不是继续在 Action 里加长 settle_timeout 装作平稳取消。