2. Control 模块架构

上手与配置见 §0；Checker / Smoother / 控制器见 §3–§5。本文只写 边界、分层、FollowPath 循环、插件加载、错误与扩展。

2.1 在导航栈中的位置

Planning ──Path──► ControllerServer ──FollowPath──► ControllerInterface
                        │                                    │
                        │◄── odom / TF ── localization       │
                        │                                    ▼
                        ├── GoalChecker / ProgressChecker   Costmap2DWrapper
                        ├── VelocitySmoother（可选）              ▲
                        ▼                                    │
                   cmd_vel ──► 底盘              map / 共享 global costmap
                        ▲
                 Navigator BT

边界	说明
上游	Navigator（FollowPath）、Planning（`Path`）、localization（位姿/odom）
地图	`map/costmap_2d`；独立 `/local_costmap` 或 `SetSharedCostmap()` 共享全局图
下游	底盘、`VelocitySmoother`（可选）
入口	`Autonomy` 构造 `ControllerServer`（`AUTONOMY_CONTROLLER`）

| 设计约束 | nav2 对齐 · ControllerInterface 插件化 · Checker 与控制器解耦 · controller.lua → ControllerOptions · Planning 出几何 Path、Control 出轨迹/时空联合（§0.7） |

2.2 分层与实现状态

Navigator → ControllerServer → ControllerInterface ──读── Costmap2DWrapper
                │                      │
                ├── GoalChecker        └── SetPlan(Path)
                ├── ProgressChecker
                └── VelocitySmoother（可选后处理）

层	组件	职责
服务	`ControllerServer`	FollowPath 循环、插件调度、TF/odom、失败容忍
算法	`ControllerInterface`	`ComputeVelocityCommands()`、`SetPlan()`
判定	`GoalChecker` / `ProgressChecker`	到达 / 进度（独立于控制器，可单测）
地图	`Costmap2DWrapper`	碰撞检测；local 或共享 global
后处理	`VelocitySmoother`	加速度界、deadband；见 §4

组件	状态	说明
`ControllerServer` 骨架	✅	构造、TF、odom、costmap
FollowPath / `ComputeControl()`	⏳	主循环 stub
控制器插件加载	⏳	接口已定，待接 PluginManager
Goal / Progress Checker	✅	算法就绪；Lua 接线 ⏳
`VelocitySmoother`	✅	算法就绪；节点接线 ⏳
控制器插件实现	❌	无具体类

节点 controller_server · 话题 cmd_vel / odom · Action FollowPath（设计意图，见 §0.16）。

2.3 FollowPath 控制循环

        sequenceDiagram
    participant N as Navigator
    participant CS as ControllerServer
    participant C as ControllerPlugin
    participant PC as ProgressChecker
    participant GC as GoalChecker

    N->>CS: FollowPath(path)
    CS->>C: SetPlan(path)
    loop @ controller_frequency
        CS->>CS: GetRobotPose()
        CS->>PC: Check(pose)
        alt 无进度
            PC-->>CS: false → FailedToMakeProgress
        end
        CS->>C: ComputeVelocityCommands(pose, vel, GC)
        C-->>CS: cmd_vel / result code
        CS->>CS: publish（可选 VelocitySmoother）
        CS->>GC: IsGoalReached(pose, goal, vel)
    end
    CS->>CS: OnGoalExit（零速、Reset）

阶段	位置	说明
初始化	`ControllerServer`	`SetPlan`、Reset checker / controller
位姿	`GetRobotPose`	odom 优先，fallback costmap TF
进度	`ProgressChecker`	`false` → `FailedToMakeProgress`
控制	`ControllerInterface`	读 costmap、路径；返回 `ControllerResultCode`
到达	`GoalChecker`	循环退出条件
收尾	`OnGoalExit`	零速、`publish_zero_velocity` 策略

循环周期 \(T=1/f_{ctrl}\)（默认 20 Hz，controller_frequency）。连续 \(t>T_{\mathrm{fail}}\) 无有效命令则终止（failure_tolerance）。流水线概览见 §0.9。

2.4 插件与配置

2.4.1 控制器插件（配置预留）

插件 ID（示例）	类型	文档
`graceful_controller`	平滑跟踪	§10
`mppi_controller`	采样 MPC	§11
`regulated_pure_pursuit`	RPP	§12

完整列表与选型见 §5。

2.4.2 加载链

controller.lua → ControllerOptions → ControllerServer
    → controller_plugin_libraries → PluginManager
    → CreateInstance<ControllerInterface>()

条目格式："graceful_controller:GracefulController" 或 "my_id:MyClass"。Goal / Progress Checker 以独立 map<id, plugin> 管理，模式同 Planning §1.4.2。

auto [id, type] = ParsePluginSpec(spec);
plugin_manager.RegisterInProcessClass<MyController>(type);
auto ctrl = plugin_manager.CreateInstance<ControllerInterface>(
    type, options, id, tf_buffer, costmap_wrapper);
controllers_[id] = ctrl;

配置字段与 API 见 §0.12–§0.14。

2.5 错误处理

异常	触发
`InvalidController`	插件 id 无效
`ControllerTFError`	TF 查询失败
`FailedToMakeProgress`	`ProgressChecker::Check` 为 false
`InvalidPath`	空路径
`NoValidControl`	控制器无效 / 连续失败超 `failure_tolerance`
`ControllerTimedOut`	costmap 未 current

ControllerResultCode（SUCCESS / NO_VALID_CMD / COLLISION / …）见 §0.14。现象排查见 §0.19。

2.6 扩展自定义控制器

继承 ControllerInterface，实现 ComputeVelocityCommands()、SetPlan() 等
AUTOLINK_PLUGIN_MANAGER_REGISTER_PLUGIN(MyController, ControllerInterface)
controller.lua 的 controller_plugins 注册 id

步骤摘要亦见 §0.17。实现优先级见 §5.9。

2.7 相关文档

§0 指南 · §3 Checker · §5 控制器 · §6 综述