Action Server 目标队列：并发 Goal 的接受、拒绝与抢占边界

导航系统正在穿过窄门，任务层几乎同时发来“去充电”；机械臂还在压装，视觉复检又提交“回到拍照位”。两个客户端都看见 Action server 在线，于是都把 goal 发了出去。真正危险的不是“同时来两个请求”，而是服务端没有明确回答三个问题：第二个 goal 被拒绝了、排队了，还是已经让第一个 goal 失效；旧目标何时停止输出；客户端收到 accepted 时，系统究竟承诺了什么。

rclcpp_action 不替应用选择调度策略。它能为多个 goal 建立独立状态机，却不会自动保证底盘只能有一个速度源、机械臂只能执行一条轨迹，也不会给接受的 goal 提供业务队列。把示例里的“每个 accepted goal 启一个 detached thread”直接搬到真机，协议上可以同时 EXECUTING，硬件端却可能出现两个线程交替写 setpoint。Action server 的难点因此不在三个回调能不能写出来，而在准入、资源所有权、终态和物理停止是否组成一份可验收的契约。

1. 先分清“到达并发”和“执行并行”

多个 goal 在短时间内到达，只说明 send-goal 服务要处理并发请求，不代表业务可以并行执行。工程上至少有三种不同容量：

协议在途容量：goal request 正在等待 goal_callback 作答，还没有 accepted 或 rejected；
已接纳容量：服务端已经接受、客户端有权等待 result，但 goal 可能仍处于 ACCEPTED；
执行容量：真正占用底盘、机械臂、GPU 或工位的 goal 数。

用 $A$ 表示尚未终态的已接纳目标数， $R$ 表示正在执行的目标数， $Q = A - R$ 表示等待目标数。若定义“队列长度为 4”，必须说明限制的是 $Q \leq 4$ ，还是包含执行中目标的 $A \leq 4$ 。两者在满载时差一个执行槽，边界测试会得到不同结果。

导航和机械臂通常属于互斥资源：同一时刻只允许一个 goal 向同一命令接口写入，故 $R = 1$ 。地图预取、轨迹碰撞检查或多个独立机械臂则可能并行，但并行度也应由资源决定，而不是由 executor 线程数决定。例如一个 goal 同时需要“左臂 + 相机”，另一个只需要“右臂”，能否并行取决于资源集合是否相交：

ca n_{r} un (g_{i}, g_{j}) = (R_{i} \cap R_{j} = \emptyset) .

若两者最终都写 /joint_trajectory_controller/joint_trajectory，即使算法部分互不干扰，也不能宣称业务并行。反过来，两个独立夹爪拥有不同控制器和安全区，仅因为它们挂在同一个 Action server 上就强制串行，也会无谓损失吞吐。策略应围绕资源图，而不是围绕“一个 action 名字”做猜测。

2. GoalHandle 生命周期决定了哪些操作合法

ROS 2 Action 对每个被接受的 goal 维护独立状态机。与队列最相关的是六个状态：

text

goal request
   ├─ REJECT ───────────────► 没有 GoalHandle，也不会有 Result
   └─ ACCEPT ─► ACCEPTED
                    ├─ execute() ─► EXECUTING ─► SUCCEEDED / ABORTED
                    └─ cancel 被接受 ──────────► CANCELING ─► CANCELED

rclcpp_action::GoalResponse 的三个值不是文案差异，而是状态迁移承诺：

返回值	客户端看到的响应	服务端随后应做什么
`REJECT`	goal 未被接受	不创建业务任务，客户端不能等待该 goal 的 result
`ACCEPT_AND_EXECUTE`	已接受	句柄按执行路径启动，适合已有执行槽
`ACCEPT_AND_DEFER`	已接受	句柄留在 ACCEPTED，调度器稍后调用 `execute()`

实现 FIFO 队列时，ACCEPT_AND_DEFER 比“先 ACCEPT_AND_EXECUTE，再让线程在条件变量上睡眠”准确。后者会把等待资源的 goal 对外标成 EXECUTING，状态监控、超时和反馈都失真；前者明确表示服务端已经承担最终交付责任，但执行尚未开始。

ServerGoalHandle<ActionT>::execute() 只能用于尚在 ACCEPTED 的 deferred goal。succeed(result) 与 abort(result)用于 EXECUTING goal；canceled(result)用于取消请求已被接受、状态已经进入 CANCELING 的 goal。尤其不能在“新 goal 到来”时直接对仍为 EXECUTING 的旧句柄调用 canceled()：CANCELED 的协议含义是外部客户端请求取消后完成取消，服务端自主替换旧任务不满足这个前提，还可能触发非法状态迁移。服务端主动抢占旧 goal，通常应以 abort(result) 收口，并在业务 Result 中给出 PREEMPTED_BY_NEW_GOAL；若系统坚持把旧 goal 变成 CANCELED，就必须由客户端发起真实 cancel 流程。

终态还有资源语义：调用任一终态方法后，不应再向该句柄发布反馈，也不应再由旧执行体写硬件。每个 goal 的终态必须只有一个所有者。调度线程、执行线程和 cancel 路径若都能调用终态方法，压力下迟早出现双重 abort()、succeed() 与 cancel 交错。

3. `goal_callback` 是准入事务，不是排队工作线程

真实 C++ server 由三个非阻塞回调组成：

cpp

using ActionT = robot_interfaces::action::MoveTo;
using GoalHandle = rclcpp_action::ServerGoalHandle<ActionT>;

server_ = rclcpp_action::create_server<ActionT>(
  this,
  "move_to",
  std::bind(&MoveServer::on_goal, this, std::placeholders::_1,
            std::placeholders::_2),
  std::bind(&MoveServer::on_cancel, this, std::placeholders::_1),
  std::bind(&MoveServer::on_accepted, this, std::placeholders::_1),
  rcl_action_server_get_default_options(),
  action_group_);

goal_callback 收到 GoalUUID 和只读 Goal，还没有 ServerGoalHandle。它适合做四类短判断：

目标字段是否合法，例如速度上限、坐标系、关节名称；
安全模式是否允许接单，例如急停、控制器切换、定位失效；
调度策略是否允许接纳，例如队列槽位、租约、优先级；
接受后是立即执行还是延后执行。

不要在这里算整条轨迹、等待 TF、调用控制器服务或等待旧 goal 停稳。官方 create_server 契约要求三个回调都非阻塞；goal_callback 占住 executor 时，cancel、状态订阅和其他 goal response 都会一起延迟。更关键的是，send-goal 响应被拖住期间，客户端不知道服务端是否已经接管任务，很难做可靠重试。

队列容量检查要按事务处理。下面这种代码在可重入回调组或多个执行线程下会超卖：

cpp

if (queue_.size() < queue_capacity_) {
  return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
}
return rclcpp_action::GoalResponse::REJECT;

检查发生在 goal_callback，真正 queue_.push_back(handle) 却只能等 accepted_callback 拿到句柄后执行。两个回调之间存在窗口：连续两个请求都看见同一个空槽并被接受。正确做法是准入时就在同一把锁下预留名额，accepted 回调只把“预留”兑现为句柄：

cpp

rclcpp_action::GoalResponse MoveServer::on_goal(
  const rclcpp_action::GoalUUID & uuid,
  std::shared_ptr<const ActionT::Goal> goal)
{
  if (!validate(*goal) || safety_state_.load() != SafetyState::kReady) {
    return rclcpp_action::GoalResponse::REJECT;
  }

  std::lock_guard<std::mutex> lock(scheduler_mu_);
  if (admitted_ >= max_admitted_) {
    return rclcpp_action::GoalResponse::REJECT;
  }

  ++admitted_;                         // 在响应 ACCEPT 前预留
  reservations_.insert(uuid);
  return has_free_resource_locked(*goal)
    ? rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE
    : rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
}

void MoveServer::on_accepted(const std::shared_ptr<GoalHandle> handle)
{
  {
    std::lock_guard<std::mutex> lock(scheduler_mu_);
    reservations_.erase(handle->get_goal_id());
    ready_.push_back(handle);           // 回调里只登记，不执行运动
  }
  scheduler_cv_.notify_one();
}

示例省略了异常恢复，但有一个不可省略的不变量：

\textit{admitted}=\textit{reserved}+\textit{accepted_waiting}+\textit{executing}.

名额只能在 goal 到达终态、或 accepted 建档失败并明确回滚时释放，不能在“从等待队列取出”时就释放，否则执行中 goal 不计入总接纳容量。若限制仅针对等待数，则把 max_waiting 与 max_running 分开建模，别复用一个含义漂移的计数器。

4. 三类队列策略的契约差异

4.1 队列满就拒绝新目标

这是边界最清楚的背压：容量已满时 goal_callback 返回 REJECT。客户端立即知道 server 没有承担该任务，可以退避、改投其他 server 或显示“设备忙”。拒绝不会产生 GoalHandle，也没有后续 Result；因此业务失败原因若只放在 Action Result 中，客户端在 reject 路径上看不到。需要区分“参数非法”“安全闭锁”“队列满”时，可增加轻量的诊断/状态接口，或让客户端结合 server 状态读取原因，不能伪造一个 accepted goal 只为返回错误码。

适合场景是指令可重试、旧任务价值更高、执行顺序必须保持。例如机械臂正在搬运易碎件时，新的复位请求不应自动打断它；导航正在通过窄门时，后台优化器发来的新巡检点也不应静默改写当前意图。

4.2 接受并排队

ACCEPT_AND_DEFER 表示“服务端已经接单，稍后执行”。从这一步开始，server 不能因为队列拥塞又静默丢弃 goal。若后来资源失效、等待超时或节点进入维护状态，应给该 handle 一个明确终态：客户端发起取消则 CANCELED；服务端内部条件使任务无法继续则 ABORTED，并带 QUEUE_TIMEOUT、RESOURCE_UNAVAILABLE 等业务码。

排队还必须定义顺序。纯 FIFO 容易被长任务阻塞；优先级队列可能让低优先级 goal 永久饥饿；“保最新”适合视觉跟踪设定，却不适合每个任务都必须执行的工单。若采用优先级 $p$ 与等待时间 $w$ 的老化策略，可写成

s (g) = p (g) + α w (g),

并明确 $α$ 的单位与上限。调度分数只决定哪个已接纳 goal 先执行，不能成为无终态删除旧 goal 的借口。

客户端还需要排队截止时间。accepted 不是 started；若客户端从 goal response 起算“执行超时”，高负载时会把合法等待误判成执行故障。较稳妥的是 Result/诊断中分别记录 accepted_stamp、execution_started_stamp，客户端定义 $T_{queue}$ 与 $T_{run}$ 两个预算。

4.3 新目标抢占旧目标

抢占适合“最新意图优先”的资源，例如遥操作目标点、跟踪对象切换或导航上层明确改派目的地。它不是简单的 current_ = new_handle。安全序列至少是：

text

接受并暂存新 goal
        │
        ▼
向旧执行体发布 PREEMPT 意图
        │
        ▼
旧执行体停止生成命令 ─► 控制器 hold/清队列 ─► ACK
        │
        ▼
旧 handle abort(PREEMPTED_BY_NEW_GOAL)
        │
        ▼
新 handle execute() ─► 开始写新命令

如果新 goal 在旧控制器队列清空前开始输出，所谓“抢占”就会变成两条轨迹交织。若等待旧任务停止失败，新 goal 不能假装正常启动：应 abort(RESOURCE_HANDOVER_TIMEOUT)，同时把旧 goal 的实际状态和安全升级路径写入诊断。对于底盘，目标切换也许允许速度连续过渡；对于机械臂焊接、抓取，通常要先退到安全段。是否允许无 hold 平滑切换，是控制契约，不应由 Action server 猜测。

5. 单目标抢占：旧 goal 由谁结束

推荐把物理资源的写权限做成“代次令牌”。每次调度新 goal 生成递增 epoch；执行循环只有持有当前 epoch 才能下发命令。抢占线程只更新期望状态，旧执行线程在下一采样点发现令牌失效，走统一的停止路径：

cpp

void MoveServer::run(
  const std::shared_ptr<GoalHandle> handle,
  const std::uint64_t my_epoch)
{
  auto result = std::make_shared<ActionT::Result>();

  while (rclcpp::ok()) {
    if (handle->is_canceling()) {
      stop_and_wait();
      result->code = ActionT::Result::CANCELED_BY_CLIENT;
      finish_once(handle, Terminal::kCanceled, result);
      return;
    }

    if (epoch_.load(std::memory_order_acquire) != my_epoch) {
      stop_and_wait();
      result->code = ActionT::Result::PREEMPTED_BY_NEW_GOAL;
      finish_once(handle, Terminal::kAborted, result);
      return;
    }

    if (!command_next_point(*handle->get_goal())) {
      stop_and_wait();
      result->code = ActionT::Result::CONTROLLER_ERROR;
      finish_once(handle, Terminal::kAborted, result);
      return;
    }

    if (reached_target()) {
      result->code = ActionT::Result::OK;
      finish_once(handle, Terminal::kSucceeded, result);
      return;
    }
  }
}

finish_once 不是为了吞异常，而是把“终态所有权”显式化：每个 GoalContext 有一个原子或受锁保护的 terminal 标志，只有首次成功认领者可以调用 succeed、abort 或 canceled。不过，原子标志不能替代状态机判断；canceled() 仍只能在句柄已经 CANCELING 时调用。

这里还要区分“停止计算”和“停止机构”。旧线程退出规划循环，不代表控制器不再消费上一批轨迹点。资源交接完成条件应至少包含：

t_{\text{new_command}}\get_{\text{old_write_revoked}}\quad\land\quadt_{\text{new_command}}\get_{\text{controller_handover_ack}}.

位置控制可用当前测量位置构造 hold；速度控制通常要求零速度并等待速度降至阈值；力控可能要切到重力补偿或安全阻抗。把三者都实现成“线程停止后 sleep 一下”既不可核对，也无法覆盖控制器延迟的长尾。

6. cancel 要同时覆盖等待中和执行中 goal

cancel_callback 返回 CancelResponse::ACCEPT 的含义只是 server 同意尝试取消。rclcpp_action 会在回调返回后把对应 goal 推进到 CANCELING，真正结束仍由应用调用 handle->canceled(result)。因此不要在 cancel_callback 内直接调用 canceled()：此时状态转换尚未由库完成，也不要阻塞等待硬件停止。

等待中 goal 与执行中 goal 的取消成本不同：

ACCEPTED、未执行：从业务 ready queue 移除，不需要碰硬件；观察到 is_canceling() 后直接构造 CANCELED_WHILE_QUEUED 结果；
EXECUTING：撤销写权限，发 hold/clear，等待控制器或测量域达到停止条件，再 canceled()；
正在资源交接：cancel 与 preempt 可能同时到达，必须由同一个 GoalContext 仲裁终态，不能一个线程 abort、另一个线程 canceled；
已经终态：取消应被拒绝或由底层返回不在可取消集合，不能重新打开状态机。

回调可以保持很短：

cpp

rclcpp_action::CancelResponse MoveServer::on_cancel(
  const std::shared_ptr<GoalHandle> handle)
{
  std::lock_guard<std::mutex> lock(scheduler_mu_);
  auto it = contexts_.find(handle->get_goal_id());
  if (it == contexts_.end() || it->second.terminal_claimed) {
    return rclcpp_action::CancelResponse::REJECT;
  }
  it->second.cancel_wakeup = true;
  scheduler_cv_.notify_one();
  return rclcpp_action::CancelResponse::ACCEPT;
}

调度器被唤醒时，库的 CANCELING 转换可能刚在回调返回后发生，因此业务线程应以 handle->is_canceling() 为最终依据，并允许下一次调度拍再处理，不能仅凭 cancel_wakeup 就调用终态方法。cancel_wakeup 是降低发现延迟的提示，不是协议状态。

队列实现常漏掉“等待中 cancel”：worker 只在资源空闲时取队首，若队首后面某个 goal 已 CANCELING，它可能一直占着接纳名额。调度器应在每次入队、cancel 唤醒和周期巡检时清理所有已取消的 deferred handle；清理顺序是在锁下从业务队列摘除，锁外发布 canceled，最后在统一终态回调里归还名额。终态发布可能触发中间件工作，不应长期持有调度锁。

7. 多目标并行不是“每个 goal 一条线程”

并行 server 需要资源调度器，而非无限线程。可以给每个 goal 计算资源向量：

d_{g} = (d_{left arm}, d_{right arm}, d_{gpu}, d_{camera})

系统容量为 $c$ ，当前已运行集合为 $E$ 。只有满足

g \in E \sum d_{g} + d_{new} \leq c

才允许新 goal 从 ACCEPTED 转 EXECUTING。布尔互斥资源用 0/1 即可，GPU 显存、工位或带宽可以使用离散配额。真实系统还要加不可组合约束，例如左右臂分别空闲，但两条轨迹的扫掠体积相交，仍不得并行。

ROS 2 官方教程为了展示 Action 生命周期，常在 accepted callback 里启动 detached thread；那是最小示例，不是拥塞控制。每个 goal 一条 detached thread 有四个问题：线程数不受队列容量约束；节点析构时难以 join；多个线程可能争同一控制器；执行线程的调度优先级和栈内存不可控。工程实现更适合一个有界 worker pool，或每个互斥硬件资源一个长期 worker。worker 从调度器领取 GoalContext，调用 execute() 后才开始业务执行。

对于混合任务，可把“重规划”和“命令执行”拆开：规划池允许多个 goal 并发准备，但真正控制槽仍为 1。此时对外是否把规划阶段算 EXECUTING 要写清。如果规划失败会直接导致任务终止，通常可在 execute() 后将 phase 反馈为 PLANNING；如果只是排队前的可丢弃预测，则不应让它持有 GoalHandle 的执行承诺。关键是同一时刻只能有一个组件拥有终态权和硬件写权。

8. Executor、Callback Group 与工作队列各管一层

Action server 本身是 executor 管理的 waitable。send goal、cancel、result 请求、状态处理都需要 executor 获得运行机会。create_server 最后一个参数可指定 callback group；传空则进入节点默认组。这里有三条常被混淆的边界：

MultiThreadedExecutor 只提供可并发调度的线程，不会让同一个 MutuallyExclusive 组里的回调并行；
callback group 只约束 executor 回调，不约束自行创建的 worker 线程；
worker pool 的容量是业务并行度，不能拿 executor 的线程数代替。

Action 回调应留在一个短回调组，控制状态订阅和 watchdog 放在另一个短回调组，规划等重活进入有界工作池。若把 stop_and_wait() 放进 cancel_callback，即使使用多线程 executor，同一互斥组中的新 goal、result 请求仍会排队；若改成可重入组却没有保护 admitted_ 和资源表，则只是把延迟问题换成数据竞争。

单线程 executor 也能做可靠 Action server，前提是三个回调都快速返回，业务由 worker 或短周期状态机推进。反之，八线程 executor 配上 accepted callback 内的阻塞执行，一样会在并发 goal 下耗尽线程。应量三段延迟：

Δ_{admit} = t_{goal response} - t_{goal request},

Δ_{dispatch} = t_{execute} - t_{accepted},

Δ_{cancel observe} = t_{worker sees cancel} - t_{cancel accepted} .

第一段反映 executor 与准入回调，第二段反映业务队列，第三段反映 cancel 被执行体采样的速度。把三者混成一个“Action 很慢”，很容易误加线程或误扩队列。

锁也要分层。scheduler_mu_ 保护 GoalContext、队列和资源所有权，只做内存状态迁移；控制器服务、TF 查询、轨迹计算、feedback 和 result 发布都在锁外。否则一个慢 DDS 写或控制器 ACK 会把 goal/cancel 回调挡在同一把锁后。资源交接可以通过 promise、事件或状态快照回报调度器，但不要在 executor 回调里持锁等 future。

9. 拒绝新目标与抢占旧目标：没有通用优胜者

两种策略的核心差异不是吞吐，而是谁承担变化成本。

维度	队列满时拒绝新 goal	新 goal 抢占旧 goal
已有任务	承诺稳定，继续执行	承诺可被服务端提前终止
新客户端	立即拿到 reject，自行退避	先被接受，再等待资源交接
硬件风险	不新增切换动作	必须验证 hold、清队列和安全过渡
结果语义	新 goal 无 Result	旧 goal 通常 ABORTED + PREEMPTED
适用意图	工单、搬运、不可重复工序	最新目标点、跟踪、人工改派

导航中的“去 B”覆盖“去 A”看似天然适合抢占，但还要考虑旧路径正处于电梯、窄门或自动门协议中。若动作不可在任意点安全中断，server 应在不可抢占区间拒绝新 goal，或先接受 deferred，再到安全点执行交接。机械臂压装更明显：保压阶段的“回零”不能直接抢占，必须先卸载力并退刀。

因此策略可以是状态相关的：

text

IDLE                 新 goal -> ACCEPT_AND_EXECUTE
RUNNING 可安全中断   新 goal -> ACCEPT_AND_DEFER，并触发旧 goal 抢占
RUNNING 不可中断     新 goal -> REJECT
队列已满             新 goal -> REJECT
安全故障             所有业务 goal -> REJECT

这里 goal_callback 只读取一份原子或受短锁保护的调度快照，真正交接由 worker 完成。不要在回调里等待“进入安全点”后才作答；等待几秒才 reject 会让客户端误以为网络失联。

如果产品要求“最后一个命令永远有效”，也要规定中间已接受 goal 的命运。把队列压缩为最新 goal 时，旧 deferred handle 不能直接从容器删除。服务端主动淘汰它们应给 ABORTED + SUPERSEDED_BEFORE_EXECUTION；若客户端同时 cancel，则由仲裁规则决定 CANCELED 是否优先。一个易解释的规则是：底层已进入 CANCELING 则以 CANCELED 收口，否则服务端抢占以 ABORTED 收口，并记录触发者 goal UUID。

10. 可观测性：让客户端知道“接单”不等于“开工”

标准状态 topic 能显示 ACCEPTED、EXECUTING、CANCELING 等状态，但产品通常还需要队列与资源信息。不要把唯一信息塞进日志。可在业务 Result 与诊断状态中提供：

goal UUID、准入策略版本与接纳时间；
入队序号、优先级、资源集合；
execution_started_stamp 与累计排队时长；
终态原因：成功、客户端取消、被新目标抢占、排队超时、资源交接超时；
抢占者 UUID 或安全状态快照；
cancel response、停止命令、控制器 ACK、result 的时间戳。

队列位置只能作为观测值，不能当保证。高优先级插队、资源兼容性和 cancel 都会改变位置。若 HMI 显示“前面 2 个”，应同时显示策略名称或估算不确定性，不能把它解释成固定开始时间。

业务 Action 定义最好让终态原因机器可读：

text

# Result 片段
uint16 OK=0
uint16 PREEMPTED_BY_NEW_GOAL=10
uint16 SUPERSEDED_BEFORE_EXECUTION=11
uint16 QUEUE_TIMEOUT=20
uint16 HANDOVER_TIMEOUT=21
uint16 CONTROLLER_ERROR=30

uint16 code
string message
unique_identifier_msgs/UUID superseded_by
builtin_interfaces/Time accepted_stamp
builtin_interfaces/Time execution_started_stamp
builtin_interfaces/Time stopped_stamp

标准 WrappedResult 的 code 表示 SUCCEEDED、ABORTED、CANCELED 等协议终态；上面的业务 Result::code 解释为什么走到该终态。二者不能互相替代。客户端应先分支处理标准结果码，再解析业务码；不能把所有“非成功”统一显示成“取消完成”。

服务端诊断至少暴露 admitted、waiting、executing、canceling、队列上限、最老等待时长和各资源 owner。核对不变量时，应始终满足：

A = Q + R + C,

其中 $C$ 是已从 ready queue 摘除、正在完成取消或资源交接但尚未终态的 goal。忽略 $C$ 会在 cancel 延迟期间提前释放容量，随后接受超过安全上限的新 goal。

11. 注入拥塞与 cancel 延迟：把边界测出来

只测“发一个 goal，最终成功”无法证明队列策略。集成测试应使用可控假执行器或硬件在环控制桥，至少提供三种注入：

execution_delay：让每个 goal 占用执行槽足够久，稳定制造满队列；
stop_ack_delay：收到 hold 后延迟 ACK，放大抢占与 cancel 的交接窗口；
drop_stop_ack：完全不回 ACK，验证超时与安全升级。

以 max_running=1、max_waiting=2 为例，同时发送四个可区分 goal。若容量定义为“一个执行 + 两个等待”，应精确观察到三个 accepted、一个 rejected；任意时刻至多一个 execute()；三个已接受 goal 都必须各有唯一终态。测试不能只统计 accepted 数，还要记录每个 UUID 的状态序列，禁止出现 ACCEPTED 直接消失、EXECUTING 两次或终态后继续 feedback。

拥塞测试矩阵可按以下顺序展开：

FIFO 基线：三个 goal 按接纳顺序进入 EXECUTING，第四个立即 reject；
并发准入：多个客户端同一屏障释放 send-goal，请求再密集也不得使 admitted > max_admitted；
等待中取消：取消队尾 goal，它应不触碰控制器、及时 CANCELED，并释放一个接纳名额；
执行中取消：cancel response 快速返回，hold/ACK/测量静止后才 CANCELED；
新 goal 抢占：旧 goal ABORTED + PREEMPTED，新 goal 只能在旧写权限撤销和 handover ACK 后 EXECUTING；
交接失败：丢弃 stop ACK，新旧 goal 都不能并发写命令，新 goal 以 HANDOVER_TIMEOUT 失败或继续保持 deferred，行为须与文档一致；
优先级老化：持续注入高优先级任务，低优先级目标仍应在定义的上界内获得执行，或明确允许饥饿并由 queue timeout 终止。

cancel 延迟要拆成可定位的时间轴：

text

t0 client 发 cancel
t1 server 进入 cancel_callback
t2 client 收 cancel response
t3 worker 观察到 is_canceling()
t4 发 hold/clear
t5 controller ACK
t6 测量域满足停止阈值
t7 handle->canceled(result)

验收分别约束 $t_{2} - t_{0}$ 、 $t_{3} - t_{2}$ 、 $t_{5} - t_{4}$ 、 $t_{7} - t_{2}$ 。在 executor 注入大量短 timer 与服务请求，用来验证协议回调仍能及时运行；在 worker 注入长规划，用来验证 cancel 采样点不会被一个不可中断函数吞掉；在控制桥注入 ACK 延迟，用来验证 server 不会提前报告 CANCELED。

还要断言命令所有权：为每条硬件命令附 goal_uuid 与 epoch，假控制器记录时间线。任意时间只能接受当前 owner 的 epoch；旧 epoch 在撤销后发来的命令必须被控制桥拒绝并计数。这样能抓住“旧线程晚写一拍覆盖新 goal”的竞态，而只看 Action 状态会漏掉它。

12. 真机落地：先写不变量，再选策略

一个可靠的 Action 调度层不需要堆很多抽象，但以下不变量必须能从代码和测试中直接找到：

goal_callback 在有界时间内完成校验与容量预留，不做轨迹、TF 和控制器等待；
accepted goal 绝不静默丢失，每个 UUID 恰有一个终态；
deferred goal 只有获得资源后才调用 execute()；
同一互斥资源任意时刻只有一个有效 owner，线程数不能改变这个事实；
client cancel 与 server preempt 使用不同终态语义：前者 CANCELED，后者通常 ABORTED + PREEMPTED；
cancel response、业务停止与物理静止是三件事，各自有时间预算；
接纳容量要包含 canceling/交接中的目标，直到真正终态才释放；
队列满、不可抢占区和安全故障都有确定的 reject/abort 行为。

导航系统可先在仿真中让底盘穿窄门时连续接收目的地，检查安全区间内 reject、离开区间后的抢占，以及 controller goal 真正切换的顺序。机械臂则在压装、保压、退刀三个阶段分别注入新 goal 和 cancel，确认每一阶段的允许策略与停止方式不同。真机验收不仅看 Result，还要同步采集关节状态、控制器命令、goal UUID、epoch 和资源 owner，才能证明协议状态与物理输出一致。

最终选择“拒绝、排队还是抢占”并没有统一答案。拒绝把背压交给客户端，承诺最清楚；排队把责任留在 server，必须处理等待超时和 deferred cancel；抢占让最新意图更快生效，却引入资源交接与旧 goal 终态。rclcpp_action 提供了 REJECT、ACCEPT_AND_DEFER、execute()、is_canceling() 和三种终态方法，足以把这三套契约实现准确，但前提是应用不把它们压扁成一个“收到就开线程”的模板。

现场再遇到并发 goal，先检查四个时间点：何时预留容量，何时获得资源，何时撤销旧写权限，何时发布旧终态。四个点若不能由 UUID 串成一条时间线，队列越长、executor 线程越多，竞态只会越隐蔽；四个点被状态机、资源令牌和注入测试钉住后，Action server 才真正拥有可解释的并发边界。

1. 先分清“到达并发”和“执行并行”

2. GoalHandle 生命周期决定了哪些操作合法

3. goal_callback 是准入事务，不是排队工作线程

4. 三类队列策略的契约差异

4.1 队列满就拒绝新目标

4.2 接受并排队

4.3 新目标抢占旧目标

5. 单目标抢占：旧 goal 由谁结束

6. cancel 要同时覆盖等待中和执行中 goal

7. 多目标并行不是“每个 goal 一条线程”

8. Executor、Callback Group 与工作队列各管一层

9. 拒绝新目标与抢占旧目标：没有通用优胜者

10. 可观测性：让客户端知道“接单”不等于“开工”

11. 注入拥塞与 cancel 延迟：把边界测出来

12. 真机落地：先写不变量，再选策略

也可以看看

3. `goal_callback` 是准入事务，不是排队工作线程