3. 目标与进度检查器

Checker 是 Control 模块的判定子系统，独立于局部控制器运行。Goal Checker 判定是否到达目标；Progress Checker 判定机器人是否在向目标前进（防卡住）。

各 Checker 实现专题见 checker/15_*–19_*（文件名前缀；与 controller/15_mpc 不同目录）。文档地图见 00_guide §0.1。

3.1 接口设计

GoalChecker

virtual bool IsGoalReached(
    const Pose& query_pose,
    const Pose& goal_pose,
    const Twist& velocity) = 0;

virtual bool GetTolerances(Pose& pose_tol, Twist& vel_tol) = 0;
virtual void Reset() = 0;

ProgressChecker

virtual bool Check(PoseStamped& current_pose) = 0;
virtual void Reset() = 0;

语义约定：

GoalChecker：true = 已到达
ProgressChecker：true = 有进度（机器人移动足够）；false = 无进度 → 触发 FailedToMakeProgress

3.2 SimpleGoalChecker

六段式专题：§15 SimpleGoalChecker（背景 → 问题 → 误差模型 → 到达条件 → 算法 1–2）。

文件：checker/simple_goal_checker.* · 默认 XY 0.25 m、yaw 0.25 rad、stateful=true。

3.3 PositionGoalChecker

§16 PositionGoalChecker · 仅 XY，忽略航向 · checker/position_goal_checker.*

3.4 StoppedGoalChecker

§17 StoppedGoalChecker · Simple + 速度停稳 · checker/stopped_goal_checker.*

3.5 SimpleProgressChecker

§18 SimpleProgressChecker · XY 位移进度 · movement_time_allowance C++ 待实现

3.6 PoseProgressChecker

§19 PoseProgressChecker · XY 或航向变化算进度 · checker/pose_progress_checker.*

3.7 Checker 对比

Checker	检测维度	Stateful	速度检测	专题	典型用途
SimpleGoalChecker	XY + Yaw	✅	❌	§15	通用导航
PositionGoalChecker	XY only	✅	❌	§16	只关心位置
StoppedGoalChecker	XY + Yaw + Vel	✅	✅	§17	精确停车
SimpleProgressChecker	XY 位移	—	❌	§18	防卡住
PoseProgressChecker	XY + Yaw 变化	—	❌	§19	含旋转进度

3.8 配置与集成

Lua 配置（controller.lua）：

goal_checker = {
    xy_goal_tolerance = 0.25,
    yaw_goal_tolerance = 0.35,
    stateful = true,
},
progress_checker = {
    required_movement_radius = 0.5,
    movement_time_allowance = 10.0,
},

与控制器配对（详见 §5.8.1、§0.9.1）：

控制器族	推荐 Goal Checker	推荐 Progress
RPP / Graceful	Simple	Simple；先转后走 → Pose
MPPI / DWB	Simple	Simple
TEB / NMPC	Stopped（对接）/ Simple	Pose（多原地转）
只到点任务	Position	Simple

Proto 定义（checker_options.proto）：

message GoalCheckerOptions {
    bool stateful = 1;
    string plugin = 2;
    double xy_goal_tolerance = 3;
    double yaw_goal_tolerance = 4;
}
message ProgressCheckerOptions {
    string plugin = 1;
    double required_movement_radius = 2;
    double movement_time_allowance = 3;
}

当前限制：C++ Initialize() 内使用硬编码默认值，标注 TODO: Load parameters from configuration。临时方案：构造后调用 SetTolerances() / SetXYGoalTolerance()。完整 Lua→Proto→C++ 接线状态见 §3.13。

3.9 在 FollowPath 中的调用时序

每个控制周期 (@ controller_frequency):
  1. progress_checker->Check(current_pose)
     └─ false → 累计无进度时间 → 超时 throw FailedToMakeProgress

  2. controller->ComputeVelocityCommands(..., goal_checker, ...)
     └─ goal_checker 传入控制器，用于控制器内部终点判定

  3. IsGoalReached()
     └─ goal_checker->IsGoalReached(pose, end_pose, velocity)
        └─ true → 退出循环

3.10 调参建议

场景	xy_tolerance	yaw_tolerance	progress radius
室内通用	0.25 m	0.35 rad	0.5 m
精密对接	0.05 m	0.1 rad	0.1 m
开阔场地	0.5 m	0.5 rad	1.0 m
原地转向任务	0.25 m	0.35 rad	0.5 m + PoseProgressChecker

注意：xy_goal_tolerance 应与 Navigator BT 中 GoalReached 节点的容差保持一致（AUTONOMY_COMMON.goal_reached_tolerance）。

3.13 Lua → Proto → C++ 接线状态

Checker 算法已实现，但配置管线与 ControllerServer 插件加载尚未闭环。下表描述当前仓库真实状态（以 control_options.cpp、checker_options.proto 为准）。

3.13.1 配置管线

config/control/controller.lua
    │  AUTONOMY_CONTROLLER.goal_checker / progress_checker
    ▼
control::LoadOptions()          ← 仅读 controller_frequency、plugins、costmap_2d_options 等
    │  ❌ 未读 goal_checker / progress_checker
    ▼
proto::ControllerOptions        ← 无 CheckerOptions 字段（checker 在独立 checker_options.proto）
    ▼
ControllerServer                ← goal_checkers_ / progress_checkers_ 容器已声明；插件实例化 ⏳
    ▼
SimpleGoalChecker::Initialize() ← TODO: Load parameters；当前硬编码 + SetTolerances() 临时方案

VelocitySmoother 同理：VelocitySmootherOptions 在 smoother_options.proto，controller.lua 尚无顶层 velocity_smoother 段；见 §4.12。

3.13.2 Lua 字段 → Proto 映射（已定义 / 待接线）

顶层 controller.lua（与 Navigator AUTONOMY_COMMON.goal_reached_tolerance 对齐）：

Lua 键（`goal_checker`）	`GoalCheckerOptions` 字段	C++ 消费	状态
`plugin`	`plugin`	插件类名 / `FindGoalCheckerId`	Lua 未写；proto 已定义
`xy_goal_tolerance`	`xy_goal_tolerance`	各 Checker `Initialize`	Lua ✅ · Load ⏳
`yaw_goal_tolerance`	`yaw_goal_tolerance`	Simple / Stopped	Lua ✅ · Load ⏳
`stateful`	`stateful`	Simple / Position	Lua ✅ · Load ⏳
—	`path_length_tolerance`	Nav2 扩展	proto 预留

Lua 键（`progress_checker`）	`ProgressCheckerOptions` 字段	C++ 消费	状态
`plugin`	`plugin`	插件类名	Lua 未写 · proto 已定义
`required_movement_radius`	`required_movement_radius`	Simple / Pose `radius_`	Lua ✅ · Load ⏳
`movement_time_allowance`	`movement_time_allowance`	Nav2 时间窗口	Lua ✅ · C++ 未实现

StoppedGoalChecker 扩展（Nav2 插件参数，Autonomy proto 尚未定义）：

Nav2 参数	C++ 成员	默认
`trans_stopped_velocity`	`trans_stopped_velocity_`	0.25 m/s
`rot_stopped_velocity`	`rot_stopped_velocity_`	0.25 rad/s

PoseProgressChecker 扩展：

参数	C++ 成员	默认
`required_movement_angle`	`required_movement_angle_`	0.5 rad

3.13.3 与 `mppi_controller` 嵌套块的关系

controller.lua 内 mppi_controller.goal_checker / progress_checker 沿用 Nav2 字符串插件名（如 nav2_controller::SimpleGoalChecker），供未来 MPPI 插件自读参数；不等于 Autonomy ControllerServer 顶层 Checker 管线。FollowPath 应以顶层 goal_checker / progress_checker 为单一真相源（待 LoadOptions 接入）。

3.13.4 工程接线清单（P1）

□ ControllerOptions 增加 checker_options 字段（或 LoadOptions 合并读取 CheckerOptions）
□ control_options.cpp 解析 goal_checker / progress_checker 字典
□ ControllerServer 构造：按 plugin 字段实例化 Checker + Initialize(options)
□ SimpleProgressChecker：实现 movement_time_allowance 计时
□ GoalCheckerOptions 扩展 stopped / pose 专有字段（或插件级子字典）
□ 单元测试：Lua 容差 → IsGoalReached 边界

临时集成（测试 / 单测）：

auto gc = std::make_shared<checker::SimpleGoalChecker>();
gc->Initialize("goal_checker", nullptr);
gc->SetTolerances(
    lua_xy, lua_yaw, lua_stateful);  // 手动对齐 controller.lua

插件选型与控制器配对见 §5.8.1。