# brain：大脑 / 小脑 协同执行框架

## 目录概览

| 组件 | 角色 | 关键文件 |
| ---- | ---- | -------- |
| CerebrumNode | 高层行为/技能序列决策 (BT) | `cerebrum_node.cpp` |
| CerebellumNode | 技能执行编排 + SMACC2 状态机 | `cerebellum_node.cpp` |
| SkillManager | 解析 YAML / 注册技能接口 | `skill_manager.cpp` |
| ActionClientRegistry | 多 Action 客户端统一封装 | `action_registry.hpp` |
| ActionServerRegistry | 多 Action 服务端统一封装 | `action_registry.hpp` |
| BTRegistry | 行为树节点/序列注册工厂 | `bt_registry.hpp` |
| Skill Action Traits | 技能动作类型元编程分派 | `skill_action_traits.hpp` |
| ExecuteBtAction | 技能序列统一 Action 定义 | `interfaces/action/ExecuteBtAction.action` |
| Stats Publisher | 技能统计发布 `/brain/skill_stats` | `cerebellum_node.cpp` |

## 架构要点（当前版本特性）

1. 行为树（BehaviorTree.CPP）在 Cerebrum 动态构建：从技能集合挑选（随机或固定序列）生成 `<Sequence>` XML，注册后 tick。
2. 每个技能生成一个 BT Action Node（名称形如 `SkillName_H`），其 `on_start` 通过统一客户端向小脑发送 ExecuteBtAction 目标（封装技能名称）。
3. 小脑（Cerebellum）收到 ExecuteBtAction 目标后将 action_name 解析为一个或多个技能（现阶段每个 goal 只含 1..N 技能列表），按顺序执行：
  - 动作型技能：通过 `SkillActionTraits` 匹配具体 Action 类型并同步 `send_and_wait`，支持超时 + 取消；同时推送结构化反馈。
  - 服务型技能（示例 VisionObjectRecognition）：直接调用并反馈结果。
4. 统一统计：累计每个技能成功 / 失败次数 + 序列成功/失败计数，定期日志并以字符串形式发布到 `/brain/skill_stats`。
5. 结构化反馈 (ExecuteBtAction.Feedback)：包含阶段(Stage)、当前技能、整体进度(Progress ∈ [0,1])、细节(Detail)。
6. 运行阶段 Stage 采用：`START` / `WAIT` / `RUN` / `END`。
7. 进度计算：
  - 序列共有 N 技能；第 i(0-based) 个技能开始时 progress≈ i/N。
  - WAIT/RUN 阶段在该技能时间窗内插值（RUN 对基于超时的估算 <1.0）。
  - END 阶段统一发布 (i+1)/N，最后一个技能 END = 1.0。
8. 超时/取消：`ActionClientRegistry::send_and_wait` 支持超时；未来可暴露 cancel 调用（当前 RUN 线程轮询完成，不主动取消）。
9. 无需为每个技能单独写冗长 if-else，trait 模板自动匹配技能 → Action 类型。
10. 严格命名常量集中于 `constants.hpp`，减少硬编码串。

## BT安装
```bash

  sudo apt-get install -y libzmq3-dev  #有的环境需要安装zmq

  git clone https://github.com/BehaviorTree/BehaviorTree.CPP.git \
  cd BehaviorTree.CPP \
  mkdir build && cd build \
  cmake .. \
  make \
  sudo make install
```

## SMACC2安装
```bash
  
  sudo apt-get install -y liblttng-ust-dev  #有的环境需要安装

  git clone https://github.com/robosoft-ai/SMACC2.git
  colcon build
```

## ExecuteBtAction.action 结构

```text
Goal:
  string action_name         # 逗号/空格/分号分隔技能序列，或单个技能

Result:
  bool success
  string message             # 汇总: Seq{A,B}: A(OK)[...], B(FAIL)[...]
  int32 total_skills
  int32 succeeded_skills

Feedback:
  string stage               # START | WAIT | RUN | END
  string current_skill
  float32 progress           # 0..1 序列整体进展
  string detail              # 附加信息 (attempt= / OK: / FAIL:原因)
```

## 反馈阶段语义

| Stage | 触发时机 | 说明 |
| ----- | -------- | ---- |
| START | 技能开始调度 | 记录基础进度基线 (i/N) |
| WAIT  | 等待对应 Action Server 可用重试 | detail=attempt=k |
| RUN   | 已发送 / 正在等待动作结束 | 基于超时线性插值进度，不超过 (i+1)/N 的 0.99 |
| END   | 技能完成（成功/失败） | 最终进度 (i+1)/N + detail=OK/FAIL:... |

## 技能动作 Traits 分派

`skill_action_traits.hpp` 定义：

```cpp
using SkillActionTypes = std::tuple<ArmSpaceControl, HandControl, LegControl, VisionGraspObject>;

// dispatch_skill_action<0, SkillActionTypes>(skillName, topic, registry,...)
```

扩展新技能：

1. 在 `interfaces/action/` 新增 Action 定义并编译。
2. 在 traits 中专门化 `SkillActionTrait<YourAction>`（success/message 适配）。
3. 将新 Action 类型加入 `SkillActionTypes` 元组。
4. 确保 YAML 中声明该技能及其接口列表（含 `.action` 标识）。

## 技能注册与 YAML

`SkillManager` 解析技能文件（默认 `share/brain/config/robot_skills.yaml`），为每个声明的动作接口注册客户端（topic 采用 CamelCase 与 snake_case 双尝试）。

## 行为树序列构建逻辑

1. 若参数 `fixed_skill_sequence` 提供 → 使用固定序列。
2. 否则随机挑选满足拥有 action 接口的技能（当前示例默认 1 个，可调整）。
3. 生成 `<Sequence>` XML 并通过 `BTRegistry` 注册。
4. 10ms 定时器 tick root；节点通过 per-node 执行状态表跟踪 in-flight / timeout。

## 超时与失败策略

| 维度 | 策略 |
| ---- | ---- |
| 单技能动作超时 | `ActionClientRegistry::send_and_wait` 等待；超时返回失败 detail=action failed |
| Server 不可用 | 最多 8 次 WAIT 重试；失败 detail=Server unavailable after retries |
| 序列失败聚合 | 任一失败若参数 `abort_on_failure=true` → 提前中断序列 |
| 行为树节点超时 | 节点 `on_running` 判定 elapsed > node_timeout_sec_ → FAIL |

## 统计发布 `/brain/skill_stats`

格式：`seq_ok=X,seq_fail=Y;SkillA=ok/fail;SkillB=ok/fail...`
（后续可演进为自定义 msg 类型与更丰富指标：平均耗时、成功率窗口等）

## 关键参数

| 参数 | 说明 | 默认 |
| default_action_timeout_sec | 动作通用超时 | 60 |
| vision_grasp_object_timeout_sec | 特定技能超时 | 120 |
| skill_timeouts | 覆盖型超时映射 Skill=sec | 空 |
| allowed_action_skills | 白名单（逗号/分号/空格） | 空(全允许) |
| fixed_skill_sequence | 固定序列 | 空(随机) |
| stats_log_interval_sec | 统计日志周期 | 60 |

## 构建与运行
```bash
colcon build --packages-select interfaces smacc2_msgs smacc2 brain
source install/setup.bash
ros2 run brain brain_node
```

## 典型日志观察点
| 日志前缀 | 意义 |
| -------- | ---- |
| [BT][on_start] | 行为树节点开始调度技能 |
| [ExecuteBtAction] Dispatch skill= | 小脑收到并调度技能 |
| [ExecuteBtAction][feedback] stage=RUN | 正在插值进度 |
| [stats] sequences success= | 定期统计输出 |

## 新增技能快速步骤
1. 定义 Action（或 Service）接口并编译。
2. 在 YAML 中添加技能条目及接口（`YourSkill.some_pkg/YourAction.action`）。
3. traits 中专门化 + 元组加入。
4. 运行后自动出现在随机/固定序列候选。

## 取消支持（Roadmap）
目前框架已在 `ActionClientRegistry` 保留 goal handle，将引入：
1. RUN 线程检测 cancel 请求（`goal_handle->is_canceling()`）。
2. 调用底层 `cancel_goal_async` 或本地标志提前结束并发布 END:FAIL:Canceled。

## 已完成的重构里程碑
| 阶段 | 内容 |
| ---- | ---- |
| Phase 1 | 统一 ExecuteBtAction 名称/常量、去除 pending 全局状态、拼写修复、事件细分 |
| Phase 2 | 超时 + 取消基础、traits 动态分派、统计 topic、结构拆分工具 |
| Phase 3 | ExecuteBtAction 结构化反馈 (stage/current_skill/progress/detail) + RUN 阶段插值 |

1. 自定义统计消息（`brain_msgs/msg/SkillStats.msg`）。
2. 反馈中加入“估计剩余时间 / 单技能耗时”。
3. 提供远程服务用于动态加载/禁用技能。
4. 更精确 goal handle ↔ skill 映射（当前简单扫描 in_flight）。
5. 多线程 executor 下互斥完善（现在统计有锁，其它结构可加读写锁）。

## 兼容性说明
本版本修改了 `ExecuteBtAction.action` 字段（Result/Feedback 增强）。外部依赖该 Action 的节点需要重新编译并适配新增字段；旧字段仍保留（success/message），保持最小破坏。

## 许可证 / 版权
示例/内部框架文件保留原版权头，外部依赖库遵循其各自 LICENSE（BehaviorTree.CPP, SMACC2, Boost）。

## FAQ 精简
| 问题 | 可能原因 | 排查 |
| ---- | -------- | ---- |
| 一直 WAIT | Action Server 未启动或主题命名不匹配 | 检查 topic / `ros2 action list` |
| RUN 不出现 | 动作执行极快或在 WAIT 就失败 | 使用真实服务器并延长执行时间 |
| progress 不到 1.0 | 未收到 END（节点超时/异常） | 查看 Cerebrum 日志/超时配置 |
| 序列提前终止 | `abort_on_failure=true` 且技能失败 | 设为 false 继续剩余技能 |

---
如需进一步扩展（自定义消息、完全取消通道或分布式多进程拆分），可在 Issue / 任务规划中补充需求。

---
## 文档注释规范（Doxygen）

所有对外（以及大多数非平凡内部）函数需使用统一的 Doxygen 模板块：

```
/**
 * @brief 使用祈使语气的一句话概要描述。
 *
 * 可选的详细说明：为什么需要、输入输出语义、约束/前置条件。
 * @param foo 参数含义；对显而易见的 getter/setter 可省略。
 * @param[out] bar 输出参数（仅在函数内被写入时使用）。
 * @return 返回值语义（void 可省略）。
 * @thread_safety 并发/线程安全说明（读/写锁策略、无锁、仅主线程等）。
 * @warning 可能造成误用或隐藏陷阱的情形。
 * @note 额外提示 / 使用建议。
 */
```

编写要点：
- 第一行 `@brief` 只写一句话；其后空一行再写补充段落。
- 只对“非平凡”参数写说明，避免“@param node 节点对象”这类噪声。
- 并发保证统一用 `@thread_safety`（在 `Doxyfile` 里通过 `ALIASES` 高亮）。
- 仅当原地修改参数才用 `@param[out]`；否则描述返回对象。
- 尽量描述“副作用 / 资源所有权 / 失败条件”，而不是重复类型信息。
- 内部复杂 helper 也需要简短说明其不变量或算法思路。
- 错误处理策略（返回 false / 抛异常 / 日志警告）尽量写清楚。

### 代码示例：ExecuteBtAction 反馈主循环

下列伪代码展示“单技能执行 + 周期性 RUN 反馈”的骨架：

```cpp
// 摘要自 CerebellumNode::ExecuteActionSkill / RunActionSkillWithProgress
bool ExecuteActionSkill(skill, topic, spec, detail, index, total, goal_handle) {
  // START：发布基础进度 (index / total)
  PublishFeedbackStage(goal_handle, "START", skill, base_progress, "");

  // WAIT：循环等待 Action Server 可用，期间多次发布 WAIT 反馈
  if (!WaitForActionServer(topic, skill, base_progress, goal_handle, detail)) {
    PublishFeedbackStage(goal_handle, "END", skill, base_progress, "FAIL:unavailable");
    return false;
  }

  // RUN：启动线程执行 dispatch_skill_action(...)；主线程按固定周期插值进度并发 RUN 反馈
  worker = std::thread([&]{ ok = dispatch_skill_action(...); finished = true; });
  while (!finished && elapsed < timeout) {
     progress = interpolate(index, total, elapsed/timeout);
     PublishFeedbackStage(goal_handle, "RUN", skill, progress, "");
     sleep(200ms);
  }
  worker.join();

  // END：最终发布 OK / FAIL 结论
  PublishFeedbackStage(goal_handle, "END", skill, final_progress, ok?"OK":"FAIL:..." );
  return ok;
}
```

目的：即便底层具体技能 Action 不提供连续反馈，外部也能得到单调递增的序列级进度，以便 UI / 上层调度展示。

### 新增已文档化 API 流程
1. 修改 / 新增头文件声明（.hpp）。
2. 添加上述 Doxygen 注释块（参考现有文件风格）。
3. 构建并开启文档：`colcon build --packages-select brain --cmake-args -DBUILD_DOCS=ON`。
4. 打开 `build/brain/docs/html/index.html` 检查别名渲染与段落格式。

### 常见错误
- 忘记空行：`@brief` 后没有空行会导致摘要与正文合并。
- 重复陈述类型：如 “@param timeout 超时时间（秒）” 可以接受，但不要仅写 “int 超时”。
- 忽略线程语义：共享状态更新必须写明锁或原子策略。
- 随意换行：超长行适当换行，但保持注释块左右对齐与一致缩进。

## Doxygen 构建

启用文档：
```bash
colcon build --packages-select brain --cmake-args -DBUILD_DOCS=ON
```
生成目录：`build/brain/docs/html/index.html`。

最小 GitHub Actions CI 示例：
```yaml
jobs:
  docs:
    runs-on: ubuntu-latest
    steps:
      - uses: actions/checkout@v4
      - name: 安装依赖
        run: sudo apt-get update && sudo apt-get install -y doxygen graphviz
      - name: 构建文档
        run: |
          colcon build --packages-select brain --cmake-args -DBUILD_DOCS=ON
      - name: 上传产物
        uses: actions/upload-artifact@v4
        with:
          name: brain-docs
          path: build/brain/docs/html
```

## CerebrumNode 机制详解

本节对大脑节点 `CerebrumNode` 的内部结构、行为树调度流程、并发模型与扩展策略做系统阐述，便于快速理解其作为“高层技能序列生成 + 行为树执行协调者”的作用。

### 1. 角色与目标
| 目标 | 说明 |
| ---- | ---- |
| 序列生成 | 基于参数（固定 / 随机）与白名单筛选技能集合，生成有序技能序列。 |
| 行为树封装 | 将技能序列转换为一串 `<SkillName>_H` 的 BT Action 节点 `<Sequence>`。|
| 统一下发 | 借助单一 `ExecuteBtAction` 将完整序列（CSV + epoch）一次性发送给 Cerebellum。|
| 状态跟踪 | 为每个 BT 节点维护 in_flight / result / progress / 超时。|
| 反馈接入 | 监听 ExecuteBtAction 反馈与结果，更新节点执行状态并保持进度单调。|
| Epoch 隔离 | 通过 epoch + 技能集合过滤陈旧反馈，避免跨序列污染。|
| 动态重建 | 定时或显式服务触发重建行为树（可配置是否允许执行期间重建）。|

### 2. 关键数据结构
| 成员 | 说明 |
| ---- | ---- |
| `action_registry_` | 统一 ActionClient 注册与发送封装。|
| `bt_registry_` | 行为树节点 & 序列工厂注册构建器。|
| `skill_manager_` | 解析与缓存技能定义（名称→接口列表等）。|
| `per_node_exec_` | `bt_type` → 执行跟踪结构（开始时间、进度、结果码、是否还在运行）。|
| `current_sequence_skills_` | 当前选定技能名称有序列表（受互斥锁保护）。|
| `active_sequence_` | 已在 BT 中激活的序列标识（“FixedSequence”/“VLMSequence”）。|
| `epoch_filter_` | 维护当前 epoch 与技能集合，供反馈过滤。|
| `bt_pending_run_` | 标记当前行为树是否需要继续 tick。|
| `bt_updating_` | 原子布尔，防止重入的 BT 重建。|

`PerNodeExecInfo` 关键字段：
| 字段 | 用途 |
| ---- | ---- |
| `in_flight` | 节点是否处于 RUNNING（尚未结束）。|
| `result` / `result_code` | 最终布尔与分类结果（SUCCESS/FAILURE/CANCELLED/UNKNOWN）。|
| `start_time` | 用于节点级超时判定。|
| `last_progress` | 保持反馈进度单调，过滤倒退。|

### 3. 参数与优先级
| 参数 | 影响 |
| ---- | ---- |
| `skills_file` | 技能 YAML 路径；空则回退默认 share 下文件。|
| `node_timeout_sec` | 单个 BT 节点（技能代理）最大运行秒数。|
| `allow_bt_rebuild_during_execution` | 执行中是否允许重建（否则跳过定时重建）。|
| `allowed_action_skills` | 白名单（逗号/分号/空格）；非空则仅保留匹配技能。|
| `fixed_skill_sequence` | 固定序列优先于随机；解析后若非空直接使用。|
| `random_skill_pick_count` | 随机模式下最多选择技能数。|

优先级：固定序列存在 → 忽略随机；白名单仅影响随机候选池；`node_timeout_sec` 对所有 BT 节点统一生效。

### 4. 生命周期与时序
1. 构造：声明参数 → 加载技能文件 → 注册全部技能 BT 节点（`RegisterSkillBtActions`）→ 注册 ExecuteBtAction 客户端（`RegisterActionClient`）→ 启动两个定时器：
  - `bt_timer_` (10ms)：`ExecuteBehaviorTree()` 持续 tick root（仅在 `bt_pending_run_` 为真）。
  - `task_timer_` (5s)：`CerebrumTask()` 触发一次序列生成 + 重建流程。
2. 重建服务：`/cerebrum/rebuild_now` 外部调用时执行：模型（选序列）→ 取消旧 goal → 重建 BT → 触发新一轮 tick。
3. 停止：BT root 返回 SUCCESS/FAILURE 时重置 `bt_pending_run_`。

### 5. 行为树节点注册机制
`RegisterSkillBtActions` 为每个技能创建 `<SkillName>_H` 类型：
| 生命周期回调 | 逻辑 |
| ------------- | ---- |
| `on_start` | 初始化节点跟踪结构；如果是序列第一个节点且 ExecuteBtAction server 可用 → 发送统一 goal。|
| `on_running` | 检查节点是否超时（`elapsed > node_timeout_sec_` 则 FAIL）；否则保持 RUNNING。|
| `on_halted` | 目前仅日志记录。|

这样一个 `<Sequence>` 中各节点只在第一次 `on_start` 时触发统一 goal（其余节点等待反馈驱动完成）。

### 6. 序列生成策略
函数 `RunVlmModel()`：
1. 若存在 `fixed_skill_sequence_` → 包装为 `FixedSequence` 注册/激活。
2. 否则调用 `PickRandomActionSkills(count)`：
  - 遍历 `skill_manager_->skills()`，过滤无 `.action` 接口技能。
  - 若白名单非空再过滤非白名单项。
  - 使用 `std::shuffle` 打乱，截断到 `random_skill_pick_count_`。
3. 将结果转为 `BTSequenceSpec` (`MakeSequenceFromSkills`) 并通过 `RegisterBtSequence` + `SetActiveSequence` 激活。

### 7. CSV 序列与 Goal 构造
`RegisterActionClient` 中 Goal builder 回调：
| 步骤 | 描述 |
| ---- | ---- |
| 获取 epoch | `epoch_filter_.epoch()`（当前行为树有效窗口）。|
| 加锁复制 `current_sequence_skills_` | 确保与重建互斥。|
| 组装 CSV | 以逗号拼接技能名放入 `goal.action_name`。|
| 返回 Goal | 统一发送给 Cerebellum 执行。|

### 8. 反馈与结果处理
| 回调 | 作用 |
| ---- | ---- |
| Feedback | 验证 `epoch_filter_.accept(epoch, skill, bt_pending_run_)`；匹配到 `per_node_exec_` 中对应 skill（通过 `skill_name`）更新 `last_progress`、`result`、`result_code`。|
| Result | 汇总成功/失败；为仍 `in_flight` 的节点补完最终结果（保证 BT 节点能完成）。|

进度单调：若收到 RUN 且 progress < `last_progress`（浮点微差外）则忽略，防倒退。

### 9. Epoch 过滤机制
重建行为树 (`UpdateBehaviorTree`) 时：
1. increment epoch。
2. 捕获当前序列技能列表并调用 `epoch_filter_.reset_epoch(new_epoch, seq_skills)`。
3. 清空 `per_node_exec_`，置 `bt_pending_run_ = true`。

反馈只要 epoch 或技能名不在集合内 → 丢弃。

### 10. per_node_exec_ & 超时
`on_start` 填充初值；`on_running` 里比较 `now - start_time` 与 `node_timeout_sec_`。到期：设置 `result=false` / `result_code=FAILURE` 返回 BT FAILURE，使 Sequence 终止（或由上层策略处理）。

### 11. 并发与线程安全
| 场景 | 处理 |
| ---- | ---- |
| 序列列表写入 | `std::lock_guard<std::mutex> lk(exec_mutex_)` 在生成/更新时保护。|
| 反馈更新节点状态 | 同一互斥锁保护遍历与写入 `per_node_exec_`。|
| 行为树重建重入 | `bt_updating_` 原子布尔 + RAII guard。|
| 运行标志 | `bt_pending_run_` 原子控制 tick。|
| epoch 计数 | 通过 `epoch_filter_`（内部原子/轻量结构）。|

### 12. 错误与失败处理
| 情况 | 行为 |
| ---- | ---- |
| ExecuteBtAction server 不可用（首节点） | 立即标记该节点 FAIL，行为树 Sequence 失败。|
| 节点超时 | `on_running` 返回 FAILURE。|
| 反馈缺失 | 节点会持续 RUNNING 直至超时或动作整体 result 回调补发。|
| 重建时旧反馈 | 被 epoch_filter 拦截。|

### 13. 扩展：新增技能
1. 在 YAML 添加技能（含至少一个 `.action` 接口）。
2. 编译生成对应接口 Action；被 `SkillManager` 自动发现并注册客户端。
3. 重建（定时或服务）后自动出现在随机候选；行为树节点自动可用。无需修改 CerebrumNode 核心代码。

### 14. 常用辅助函数梳理
| 函数 | 作用 |
| ---- | ---- |
| `PickRandomActionSkills` | 随机挑选动作技能池。|
| `MakeSequenceFromSkills` | 技能名 → `BTSequenceSpec`。|
| `BuildXmlFromSequence` | 诊断展示生成 XML（调试用）。|
| `ParseListString` | 解析参数列表（逗号/空格/分号）。|
| `ParseResultDetail` | detail 字符串 → `ExecResultCode`。|
| `CancelActiveExecuteBtGoal` | 取消当前统一执行 goal。|

### 15. 示例执行时间线
```
T0  定时器触发 CerebrumTask → 运行 RunVlmModel 选序列 [A,B]
T0+ CancelActiveExecuteBtGoal (无旧 goal) → UpdateBehaviorTree → epoch=12
T0+ bt_pending_run_=true → 10ms 定时器开始 tick root
T1  节点 A on_start → 发送 ExecuteBtAction(goal: "A,B", epoch=12)
T1+ 等待 Cerebellum 反馈；A on_running 维持 RUNNING
T2  收到 A END OK 反馈 → A 节点成功 → BT 继续到 B
T3  B on_start → 已是后续节点，不再发送 goal（沿用同一次序列）
T4  收到 B END FAIL → Sequence 失败 → root 返回 FAILURE → bt_pending_run_=false
```

### 16. 设计取舍与理由
| 需求 | 方案 | 原因 | 代价 |
| ---- | ---- | ---- | ---- |
| 低耦合技能列表变更 | CSV+epoch 单一 Action 传输 | 简化网络/接口数量 | 需解析/过滤反馈 |
| 行为树可观测性 | per_node_exec_ 状态表 | 易日志与调试 | 需互斥锁维护 |
| 反馈抗陈旧 | epoch + 技能集合校验 | 简洁快速 | 跨 epoch 丢弃旧技能晚到反馈 |
| 最小侵入扩展 | 自动注册 BT 节点 | 新技能零修改主逻辑 | 节点命名规则固定 `_H` |
| 进度单调 | 过滤回退 progress | 避免 UI 抖动 | 不显示真实回退场景 |

### 17. 后续增强
1. 更细粒度的 per-node 状态（排队、等待 server、执行中、被取消）。
2. 结合行为树黑板记录节点执行统计历史（平均时长、成功率）。
3. 支持多序列并行（需要多 Action 或自定义 goal id 映射）。
4. 将 per_node_exec_ 暴露为诊断 topic（如 `/brain/cerebrum_nodes`）。
5. 插入 “预处理 / 过滤” 节点（如安全约束、资源仲裁）。

---

## CerebellumNode 机制详解

本节全面描述小脑节点（`CerebellumNode`）的内部结构、执行路径、并发模型与统计/反馈策略。它承担“具体技能执行器 + 序列反馈聚合 + 统计汇总”三大角色，是大脑行为树（Cerebrum）与底层原子技能 Action/Service 之间的桥梁。

### 1. 组件职责分层

| 层次 | 责任 | Cerebellum 侧落点 |
| ---- | ---- | ----------------- |
| 序列入口 | 接收统一 ExecuteBtAction 目标（包含技能 CSV + epoch） | `SetupExecuteBtServer` + `RunExecuteBtGoal` |
| 技能解析 | 拆解 CSV，校验技能定义 | `ValidateAndParseGoal` / `ParseSkillSequence` |
| 超时决策 | 计算单技能执行窗口 | `GetTimeoutForSkill` + skill_timeouts_ map |
| 动作执行 | 通过注册的 ActionClient 发送并等待 | `ExecuteActionSkill` / `RunActionSkillWithProgress` |
| 服务执行 | 同步调用支持的服务接口 | `ExecuteServiceSkill` |
| 反馈生成 | START / WAIT / RUN / END 四阶段发布 | `PublishFeedbackStage` + 各执行函数内循环 |
| 统计汇总 | 成功/失败计数 & 序列聚合 | `RecordSkillResult` / `LogStatsPeriodic` |
| 终止策略 | 失败是否提前中断整序列 | `abort_on_failure_` 在 `RunExecuteBtGoal` 中判定 |
| 结果合成 | 序列总结 message 组织 | `FinalizeExecuteBtResult` |

### 2. 生命周期概览

1. 节点启动：构造函数中初始化 ActionServerRegistry / ActionClientRegistry / SkillManager；声明参数并加载技能 YAML；创建统计定时器；注册 ExecuteBtAction 服务端；可选启动 SMACC2 状态机。
2. 接收 Goal：`on_goal` 回调根据 epoch 递增规则决定拒绝/接受；触发状态机事件 `EvGoalReceived`。
3. 执行序列：`RunExecuteBtGoal` 解析技能列表，循环按顺序逐个执行；每个技能内包含等待 server、执行、反馈插值、结束判定。
4. 更新统计：每个技能结束后调用 `RecordSkillResult`；整体结果更新序列成败计数。
5. 发布结果：`FinalizeExecuteBtResult` 汇总 message（含每技能 OK/FAIL 及 detail），调用 succeed/abort。
6. 定期输出：`LogStatsPeriodic` 定时器发布/打印全局统计串。

### 3. ExecuteBtAction 反馈语义细化

| 阶段 | 触发点 | 进度计算 | detail 示例 | 失败路径 |
| ---- | ------ | -------- | ----------- | -------- |
| START | 技能进入执行逻辑前 | i / N | 空 | - |
| WAIT  | Server 每次不可用重试 | 固定基线 (i / N) | attempt=1..k | 若超出重试次数直接 END=FAIL |
| RUN   | 已发送 Action 并在超时窗口内 | i + 子进度 / N (上限 < (i+1)/N) | 空或插值内部不填 | 超时后线程结束，END=FAIL |
| END   | 技能成功/失败完成 | (i+1)/N | OK / OK:found pose / FAIL:reason | 已最终化 |

设计目标：即便底层 Action 本身不推送 feedback，也能通过 RUN 插值为上层（Cerebrum / UI）提供平滑进度感知。

### 4. 单技能动作执行时序

```
ExecuteActionSkill
  ├─ START 反馈
  ├─ WaitForActionServer (循环 WAIT 反馈 / 最多 8 次)
  ├─ 计算超时窗口 timeout_sec
  └─ RunActionSkillWithProgress
       ├─ 启动工作线程 dispatch_skill_action (阻塞发送+等待结果)
       ├─ 主线程 200ms 周期：计算 phase = elapsed/timeout，发布 RUN 反馈
       ├─ 工作线程返回：设置 finished
       └─ 结束汇总 local_detail → 返回 ok
```

`dispatch_skill_action` 利用模板递归在 `SkillActionTypes` 元组中匹配技能名并调用对应类型安全的 `send_and_wait`，抽象出不同 Action 的 goal / result 字段差异。

### 5. 服务型技能执行

目前示例仅支持 `VisionObjectRecognition`：
1. 检索 typed service client；等待可用（2s）。
2. 发送请求并同步等待（5s 限时）。
3. 根据 `found` 字段判定成功与 detail（`found pose` / `Not found`）。
4. 统一通过 START → END 两阶段反馈（无 RUN 插值）。

可扩展思路：抽象 `ServiceSkillTrait`，与 Action 分派类似以支持多服务类型。

### 6. 超时与重试策略

| 粒度 | 策略 | 相关函数 / 参数 |
| ---- | ---- | --------------- |
| Server 可用性 | 8 次重试，250ms 间隔，50ms is_server_available 等待 | `WaitForActionServer` |
| 动作执行 | 线性插值 RUN，超过 timeout 判 FAIL | `RunActionSkillWithProgress` + `GetTimeoutForSkill` |
| 服务执行 | 单次 wait_for_service(2s) + spin_until_future(5s) | `ExecuteServiceSkill` |
| 动作超时表 | skill_timeouts_ > vision_grasp_object_timeout > default | `GetTimeoutForSkill` |

### 7. 并发与线程安全

| 数据 / 操作 | 并发措施 | 说明 |
| ------------ | -------- | ---- |
| 统计计数 (success/fail maps) | `std::mutex stats_mutex_` | 在 `RecordSkillResult` 与 `LogStatsPeriodic` 加锁 |
| 进度反馈发布 | 单线程主执行循环调用 | worker 线程只设原子 flag，不直接 publish |
| 工作线程完成信号 | `std::atomic<bool> finished` | 主循环轮询 200ms |
| epoch 切换 | `current_epoch_` (atomic) | goal on_goal 时递增；反馈携带 epoch 供上层过滤 |
| 行为树并发 | 不在 Cerebellum 内创建多 executor 线程 | 由外层 ROS 2 executor 控制 |

当前不对 `action_clients_` 做细粒度锁，假设注册阶段已完成且执行期只读；若未来动态增加/卸载客户端需引入 RW 锁或线程安全封装。

### 8. 失败分类与消息合成

序列结果消息格式示例：
```
Seq{ArmSpaceControl,VisionGraspObject}: ArmSpaceControl(OK)[pose reached], VisionGraspObject(FAIL)[Not found]
```
组装逻辑：循环累积 `skill(OK|FAIL)[detail]`，失败时在日志打印并依据 `abort_on_failure_` 决定是否跳出。

### 9. 与 SMACC2 的交互

若未定义 `BRAIN_DISABLE_SM`：
- 接收新 goal → `EvGoalReceived`。
- 单技能成功 → `EvExecutionFinished`。
- 单技能失败 → `EvExecutionFailed`。

状态机可据此进行更高层的恢复、统计或模式切换（例如失败后进入恢复状态）。当前实现保持事件投递简单，不阻塞执行主线。

### 10. 可扩展方向

1. 真正的取消支持：在 RUN 循环检查 `goal_handle->is_canceling()`，触发底层 `cancel_goal_async`，并发布 END:FAIL:Canceled。
2. 细粒度反馈：透传底层 Action 自身的 feedback 字段（若存在），与插值融合。
3. 统计类型升级：发布结构化自定义消息（包含平均耗时、成功率、最近窗口数据）。
4. 多并发序列：支持多个 ExecuteBtAction 并行（需拆分 epoch / 反馈通道或使用 goal UUID 区分）。
5. 失败重试策略：对特定技能定义最大重试次数与指数退避。
6. 服务技能抽象：与 Action 一致的 trait + 统一执行包装。
7. 观测指标：Prometheus exporter 暴露计数与耗时直方图。

### 11. 关键函数对照表

| 函数 | 作用概述 |
| ---- | -------- |
| `SetupExecuteBtServer` | 注册 ExecuteBtAction 服务端及回调 |
| `RunExecuteBtGoal` | 序列主调度循环：技能遍历、执行、统计、结果聚合 |
| `ValidateAndParseGoal` | 校验 goal 与解析技能 CSV |
| `ParseSkillSequence` | 分隔符拆分 + 去 `_H` 后缀 |
| `ExecuteSingleSkill` | 统一入口，分派到 Action 或 Service 执行路径 |
| `ExecuteActionSkill` | 包装启动、等待 server、构造超时、调用运行函数 |
| `WaitForActionServer` | Server 可用性重试 + WAIT 反馈 |
| `RunActionSkillWithProgress` | 启动行动作线程 + RUN 插值反馈循环 |
| `ExecuteServiceSkill` | 同步服务调用 + START/END 反馈 |
| `PublishFeedbackStage` | 构造并发送反馈消息对象 |
| `GetTimeoutForSkill` | 计算最终超时（优先级链） |
| `RecordSkillResult` | 原子更新 per-skill 成败计数 |
| `LogStatsPeriodic` | 周期发布/打印统计字符串 |
| `FinalizeExecuteBtResult` | 组织 result message & succeed/abort |

### 12. 典型执行示例（时间线）

```
T0  : Goal 接收 (epoch=5) → 接受并进入 RunExecuteBtGoal
T0+ : 解析技能: [ArmSpaceControl, VisionGraspObject]
T1  : ArmSpaceControl START
T1+ : ArmSpaceControl WAIT attempt=1..n (server 暂不可用)
T2  : ArmSpaceControl RUN (周期进度上升 0.0→~0.49)
T4  : ArmSpaceControl END OK (进度=0.5)
T4+ : VisionGraspObject START
T5  : VisionGraspObject RUN（若是 Action；当前示例为 Service 则直接调用）
T6  : VisionGraspObject END FAIL:Not found (进度=1.0)
T6+ : 序列结果汇总 & 发布 Result(success=false)
T6+ : 统计串更新 & 定时器下一周期打印
```

### 13. 设计取舍

| 目标 | 方案 | 取舍 |
| ---- | ---- | ---- |
| 统一入口 | ExecuteBtAction 聚合 | 简化上层调度；需解析 CSV/epoch |
| 平滑进度 | RUN 插值 + 超时窗口 | 不依赖底层 feedback；进度为近似值 |
| 易扩展 | trait 模板分派 | 新增类型需专门化 trait |
| 统计简洁 | 文本串发布 | 解析不结构化；后续升级定制 msg |
| 线程简单 | 单 worker + 轮询 | 200ms 粒度有限；高精度需事件驱动 |
| 超时灵活 | 三层优先级 + map | map 维护需参数管理规范 |

---
