1. Planning 模块架构

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

1.1 在导航栈中的位置

Navigator ──GetPlan──► PlannerServer ──CreatePlan──► GlobalPlanner 插件
                           │                              │
                           │                              ▼
                           │                    Costmap2DWrapper（快照）
                           ▼
                      Path ──► Control
                           ▲
                    map 更新 /global_costmap

边界

说明

上游

Navigator（compute_path_to_pose / GetPlan）

地图

map/costmap_2d，/global_costmap

下游

Control、Visualization

入口

Autonomy 构造 PlannerServer（AUTONOMY_PLANNER）

设计约束：nav2 对齐 · GlobalPlanner 插件化 · planner.lua → PlannerOptions · 规划前 复制 costmap 快照（不长期持锁）。

1.2 分层与组件

Navigator → PlannerServer → GlobalPlanner ──读快照── Costmap2DWrapper
                │                │
                └─ 后处理 ───────┘ PathSimplifier / SimpleSmoother（可选）

层

组件

职责

服务

PlannerServer

GetPlan / IsPathValid、插件调度、TF、结果码→异常

算法

GlobalPlanner

CreatePlan(start, goal, plan, cancel_checker)

地图

Costmap2DWrapper

共享 wrapper；插件内 lock → copy char map → unlock

后处理

PathSimplifier / SimpleSmoother

见 §0.6

节点 planner_server · 地图 /global_costmap · 服务 is_path_valid（constants.hpp）。

GlobalPlanner 约定：起终点可任意 frame（变换到 costmap 系）；cancel_checker()==true 时返回 PLANNER_CANCELED；返回 PlannerResultCode 而非 bool。

1.2.1 地图层 — Costmap2DWrapper

  • 规划线程只读快照，与地图更新线程解耦（详见 §1.5）

  • 起点格常强制 FREE_SPACE，避免脚下被标障

  • 代价值语义见 §0.4；图层见 Map · Costmap2D

1.3 单次规划流程

        sequenceDiagram
    participant N as Navigator
    participant PS as PlannerServer
    participant GP as GlobalPlanner
    participant CM as Costmap2DWrapper

    N->>PS: GetPlan(start, goal, planner_id)
    PS->>PS: TF → global_frame
    PS->>GP: CreatePlan(..., cancel_checker)
    GP->>CM: lock → copy → unlock
    GP->>GP: 搜索
    GP-->>PS: Path + result code
    opt 后处理
        PS->>PS: Simplify / Smooth
    end
    PS-->>N: Path 或异常

阶段

位置

说明

坐标变换

PlannerServer

起终点 → costmap global_frame

地图快照

GlobalPlanner

加锁复制，搜索阶段只读

搜索

插件

NavFn / Dijkstra / Theta*，见 §3–§5

后处理

PlannerServer

path_simplify_epsilon、auto_smooth_after_plan

返回

PlannerServer

非 SUCCESS → 异常（§1.5）

1.3.1 NavFn 势场约定

NavFn 系插件代码 API 与用户语义对调，文档统一如下：

用户语义

代码

含义

起点 \(q_s\)

setGoal + initCost(0)

势场零点

终点 \(q_g\)

setStart

传播终止 / 路径提取起点

路径

calcPath

自 \(q_g\)（或 \(q^*\)）梯度跟踪至 \(q_s\)

算法细节：§3 NavFn §5 · §4 Dijkstra §4 · §5 Theta* §5。

1.4 插件与配置

1.4.1 内置插件

插件 ID

C++ 类

说明

navfn_planner

NavfnPlanner

NavFn，可选 A*

dijkstra_planner

DijkstraPlanner

强制 Dijkstra

theta_star_planner

ThetaStarPlanner

Theta*

1.4.2 加载链

planner.lua → PlannerOptions → PlannerServer
    → planner_plugin_libraries → PluginManager::LoadPlugin(xml)
    → CreateInstance<GlobalPlanner>()

条目格式："navfn_planner" 或 "my_id:MyClass"。字段说明与 API 见 §0.5。

1.5 错误处理与并发

码

结果

异常

0

PLANNER_SUCCESS

—

51

PLANNER_CANCELED

PlannerCancelled

54 / 55

PLANNER_BLOCKED_*

StartOccupied / GoalOccupied

56

PLANNER_NO_PATH_FOUND

NoValidPathCouldBeFound

57

PLANNER_PAT_EXCEEDED

PlannerTimedOut

59

PLANNER_TF_ERROR

PlannerTFError

61

PLANNER_INVALID_PLUGIN

InvalidPlanner

场景

策略

地图更新 vs 规划

快照复制，搜索不持 costmap 锁

多插件

共享 wrapper，算法状态各自独立

取消

每 terminal_checking_interval（默认 5000）步检查 cancel_checker

现象排查见 §0.8。

1.6 扩展自定义规划器

  1. 继承 GlobalPlanner，实现 CreatePlan()

  2. AUTOLINK_PLUGIN_MANAGER_REGISTER_PLUGIN(MyPlanner, GlobalPlanner)

  3. planner.lua 注册 id + XML 库路径

class MyPlanner : public common::GlobalPlanner {
    uint32 CreatePlan(const PoseStamped& start, const PoseStamped& goal,
                      Path& plan, std::function<bool()> cancel_checker) override;
};
AUTOLINK_PLUGIN_MANAGER_REGISTER_PLUGIN(MyPlanner, GlobalPlanner);

步骤摘要亦见 §0.7。

1.7 相关文档