1. Navigator 模块架构

上手与配置见 §0；BT 算法见 §2–§5。本文只写 边界、分层、数据流、插件加载、状态与扩展。

---

1.1 在导航栈中的位置

Autonomy / Bridge ──Goal──► BtNavigator ──BT tick──► Planner / Control / Map
                                │
                                ▼
                         navigate_to_pose.xml

边界	说明
上游	system::Autonomy、Bridge、autolink Action Client
下游	planning、control、map、transform
入口	NavigatorOptions（navigator.lua）· NavigateToPose()
不负责	路径搜索、局部控制律、定位算法

设计约束：nav2 对齐 · 编排不实现算法 · common.lua 三模块共享帧/容差 · 52 BT 插件 .so · 局部生存增强（TF 丢失时不立即失败）。

---

1.2 实现状态

组件	状态	说明
NavigatorOptions / options.cpp	✅	Lua → Protobuf 完整
NavigatorInterface	✅	生命周期接口
BehaviorTreeNavigator 模板	✅	头文件完整，依赖 BtEngine
NavigatorMuxer	✅	互斥调度
BT XML（单点/多点）	✅	含局部生存模式
navigator.lua（52 插件清单）	✅	与 CMake 目标对齐
BtEngine / BtActionServer / BtContext	⏳	头文件引用存在，实现待恢复
BT 插件 .so（52 个）	⏳	源码待从历史 tasks 迁回
BtNavigator 顶层编排	⏳	设计完成
NavigateToPoseNavigator / NavigateThroughPosesNavigator	⏳	设计完成
Autonomy BT 接线	⏳	当前 use_bt_navigation_ = false
直驱规划路径	✅	NavigateDirectToPose → GetPlan

当前阶段：6 个源文件 + 配置/BT XML 已就绪；完整 BT 栈尚待迁回。system::Autonomy 以直驱模式验证规划链路。

1.3 分层架构

navigator 模块分为 应用 → BtNavigator → BtEngine → BT 插件 → 子系统 五层。

应用层 Autonomy / Bridge / nav_test 外部调用方，不隶属于 navigator 包

对外接口

NavigateToPose() NavigateThroughPoses() navigate_to_pose Action

Goal / Cancel / Feedback

编排层 BtNavigator 模块唯一对外服务入口 · navigator 节点

核心职责

• 持有两个 BehaviorTreeNavigator 实例
• NavigatorMuxer 互斥调度
• 黑板默认值注入（PopulateBlackboardDefaults）
• Feedback 发布与 Result 错误码映射

具体 Navigator

NavigateToPoseNavigator NavigateThroughPosesNavigator

RunWithGoal / tick

引擎层 BtEngine + BtActionServer + BtContext BehaviorTree.CPP 工厂 · 100 Hz tick 循环

BtEngine

• 加载 52 个插件 .so
• 解析 BT XML → Tree
• BehaviorTreeFactory 注册表

BtActionServer

• Goal / Preempt / Cancel 回调
• Run() tick 主循环
• 黑板与 autolink 节点绑定

BtContext

planner controller tf_buffer navigator_options

Action / Service 调用

插件层 BT 节点插件（52）

类别

24 Action 16 Condition 3 Control 7 Decorator

代表节点

ComputePathToPose FollowPath RecoveryNode RateController

<div class="plan-arch-link">
  <span class="plan-arch-link-arrow">◄</span>
  <span class="plan-arch-link-text">调用<br/>子系统</span>
  <span class="plan-arch-link-arrow">►</span>
</div>

<div class="plan-arch-layer plan-arch-post">
  <div class="plan-arch-header">
    <span class="plan-arch-badge">子系统层</span>
    <span class="plan-arch-title">Planning / Control / Map / TF</span>
  </div>
  <div class="plan-arch-body">
    <div class="nav-body-block">
      <div class="nav-body-label">服务入口</div>
      <div class="nav-chip-list">
        <span class="nav-chip">PlannerServer</span>
        <span class="nav-chip">ControllerServer</span>
        <span class="nav-chip">clear_costmap</span>
        <span class="nav-chip">is_path_valid</span>
        <span class="nav-chip">transform::Buffer</span>
      </div>
    </div>
  </div>
</div>

输出：导航 Result + Feedback

1.3.1 编排层 — BtNavigator

BtNavigator 对标 nav2_bt_navigator::BtNavigator，职责：

职责	实现要点
生命周期	构造时创建 BtEngine、加载插件、实例化两个 Navigator
配置注入	从 NavigatorOptions 读取帧、容差、BT XML 路径
互斥	共享 NavigatorMuxer，拒绝并发导航
黑板初始化	PopulateBlackboardDefaults() 写入 goal_reached_tol 等
autolink	enable_autolink_action_servers 控制 Action Server 注册

1.3.2 引擎层 — BehaviorTreeNavigator<ActionT>

模板基类（common/behavior_tree_navigator.hpp）封装：

template <typename ActionT>
class BehaviorTreeNavigator {
    bool StartWithGoal(GoalPtr goal, const std::string& tree_xml = "");
    bool IsRunning() const;
    bool Cancel();
protected:
    virtual bool OnGoalReceived(GoalPtr goal) = 0;
    virtual void OnLoop() = 0;       // 更新 current_pose → Feedback
    virtual void OnPreempt(GoalPtr goal) = 0;
    virtual void OnCompleted(ResultPtr result, RunStatus status) = 0;
};

构造时创建 BtActionServer，绑定 Goal/Loop/Preempt/Completed 回调，并将 autolink 节点写入黑板。

1.3.3 插件层

52 个 BT 节点以独立 .so 注册，命名规则：

autonomy_behavior_tree_{category}_{stem}

详见 05_bt_plugins.md。

1.4 导航数据流

1.4.1 BT 单点导航时序

        sequenceDiagram
    participant App as Autonomy / Client
    participant BN as BtNavigator
    participant NTP as NavigateToPoseNavigator
    participant BT as BtActionServer
    participant PS as PlannerServer
    participant CS as ControllerServer
    participant TF as transform::Buffer

    App->>BN: NavigateToPose(goal)
    BN->>NTP: StartWithGoal(goal)
    NTP->>BT: RunWithGoal(goal, navigate_to_pose.xml)
    loop 每 10 ms tick
        BT->>TF: TransformAvailable(map, base_link)
        alt TF 可用
            BT->>PS: ComputePathToPose (5 Hz)
            PS-->>BT: path
            BT->>PS: IsPathValid(path)
            BT->>CS: FollowPath(path) [RUNNING]
        else TF 丢失
            BT->>CS: DriveOnHeading / Spin / BackUp
        end
        BT->>NTP: OnLoop() → Feedback
    end
    BT-->>NTP: SUCCESS / FAILURE
    NTP-->>App: Result
    

1.4.2 直驱模式（当前默认）

        sequenceDiagram
    participant App as Autonomy
    participant PS as PlannerServer
    participant TF as transform::Buffer

    App->>TF: GetRobotPose / TransformPose
    App->>PS: GetPlan(start, goal)
    PS-->>App: Path
    App->>App: NotifyPath(path)
    Note over App: 不调用 FollowPath
    

1.4.3 多点巡航

NavigateThroughPoses(goals[])
  → NavigateThroughPosesNavigator
  → navigate_through_poses.xml
  → ComputePathThroughPoses (10 Hz)
  → SmoothPath → IsPathValid → FollowPath
  → RecoveryNode(retries=6)

与单点树差异：无局部生存模式、无 ReactiveFallback GoalReached 前置、规划频率 10 Hz。

1.5 配置管线

config/common.lua                    ← 单一事实来源
        │
        ▼
config/navigator/navigator.lua
        │
        ▼
LuaParameterDictionary
        │
        ▼
navigator::LoadOptions()  →  proto::NavigatorOptions
        │
        ├── global_frame / robot_base_frame
        ├── bt_loop_duration (10 ms)
        ├── local_survival_timeout (120 s)
        ├── goal_reached_tolerance (0.25 m)
        ├── plugin_lib_names (52)
        ├── navigate_to_pose.behavior_tree_file
        └── navigate_through_poses.behavior_tree_file
        │
        ▼
BtNavigator(options)  [待恢复]
        │
        ├── BtEngine::LoadPlugins()
        ├── PopulateBlackboardDefaults()
        └── NavigateToPoseNavigator / NavigateThroughPosesNavigator

NavigatorOptions proto 主要字段：

字段	类型	说明
bt_loop_duration	uint32	tick 周期 ms，默认 10
default_server_timeout	uint32	子 Action 超时 ms
local_survival_timeout	double	局部生存秒数
plugin_lib_names	repeated string	BT 插件库名
navigate_to_pose	BehaviorTreeNavigatorOptions	enable + XML 文件

1.6 状态机

1.6.1 NavigatorInterface 生命周期

        stateDiagram-v2
    [*] --> kIdle
    kIdle --> kRunning : StartWithGoal()
    kRunning --> kCompleted : BT SUCCESS
    kRunning --> kFailed : BT FAILURE / 超时
    kRunning --> kCanceled : Cancel()
    kCompleted --> kIdle : 复位
    kFailed --> kIdle : 复位
    kCanceled --> kIdle : 复位
    kIdle --> kShutdown : Shutdown()
    kRunning --> kShutdown : Shutdown()
    

1.6.2 navigate_to_pose 模式切换

                    ┌─────────────┐
                    │ GoalReached?│──YES──→ SUCCESS
                    └──────┬──────┘
                           NO
              ┌────────────┴────────────┐
              ▼                         ▼
     TF available?              TF NOT available
              │                         │
              ▼                         ▼
        GlobalMode              LocalSurvivalMode
     (PipelineSequence)       (RoundRobin + relocalize)
              │                         │
              │    TF restored ◄────────┘
              │                         │
              └──── failure ────────────┘
                           ▼
                  NavigationRecovery (×8)

1.6.3 NavigatorMuxer 互斥

同一时刻仅允许一个 Navigator 活跃：

navigate_to_pose RUNNING  →  navigate_through_poses Goal REJECTED
navigate_through_poses RUNNING  →  navigate_to_pose Goal REJECTED

1.7 与系统其他模块的集成

config/autonomy.lua
  include navigator/navigator.lua
        │
        ▼
system::Autonomy
  navigator_options_ = CreateOptions(...)
  use_bt_navigation_ = false  (当前)
        │
        ├── planner_  ← ComputePathToPose / GetPlan
        ├── controller_  ← FollowPath / Spin / BackUp
        ├── tf_buffer_  ← TransformAvailable
        └── map/costmap  ← IsPathValid / ClearEntireCostmap

模块	集成点
planning	ComputePathToPose、IsPathValid
control	FollowPath、恢复原语、GoalChecker
map/costmap_2d	清图服务、路径碰撞检测
localization	间接通过 TF 影响 TransformAvailable
transform	Buffer::canTransform
Bridge	gRPC 下发 Goal（待 navigator_stub）

1.8 线程与并发

场景	策略
BT tick 线程	BtActionServer::Run() 独立线程或主循环，周期 10 ms
Navigator 互斥	NavigatorMuxer 互斥锁
子 Action 调用	同步等待或 async + cancel_checker
TF 查询	transform::Buffer 线程安全读
Costmap vs 规划	子系统内部加锁，BT 不直接操作 costmap

取消语义：Cancel() → BtActionServer::Cancel() → 中止 FollowPath / ComputePath → BT 树 HALTED → kCanceled。

1.9 扩展指南

1.9.1 新增 BT 节点插件

1. 在 behavior_tree/plugins/{category}/ 实现节点

2. 继承 BtActionNode / BtConditionNode / BtControlNode / BtDecoratorNode

3. BT_REGISTER_NODES(factory) 注册

4. CMake 添加 .so 目标

5. 更新 navigator.lua 的 plugin_lib_names

6. 更新 autonomy_tree_nodes.xml

1.9.2 新增 Navigator 类型

1. 定义 Action proto（Goal/Feedback/Result）

2. 继承 BehaviorTreeNavigator<ActionT>

3. 实现 OnGoalReceived（写黑板）、OnLoop（Feedback）、OnCompleted（Result）

4. 在 BtNavigator 中注册

5. 编写默认 BT XML

1.9.3 自定义 BT XML

复制 navigate_to_pose.xml，修改后：

• 在 navigator.lua 改 behavior_tree_file，或

• Goal 的 behavior_tree 字段运行时指定

1.10 相关文档

• Navigator 导航编排指南

• 数学原理

• 使用指南

• 行为树引擎

• 单点导航 BT

• Planning 架构