4.4 KiB
粗路径固定案例使用实时 AMR 位姿设计
背景
当前 粗路径规划-显式空图、粗路径规划-单矩形绕行、粗路径规划-多来源障碍、粗路径规划-缓存命中、粗路径规划-倒车换向 和 粗路径规划-无解 使用工厂中写死的起点、终点、地图边界和障碍物坐标。只有手动的 粗路径规划 测试读取当前 AMR 位姿。
这使固定案例无法从当前机器人位置和朝向验证规划,且 AMR 位于固定地图范围外时会直接得到无意义的越界失败。
目标
所有 粗路径规划-* 固定案例都以一次冻结的当前 AMR 位姿为起点。每个案例保留原有的相对地图、目标、障碍和方向约束,从而在现场位置验证相同的规划问题。
范围
本变更只调整 Clumsy MovementTest 中固定场景的输入创建方式、诊断显示、测试和文档。
不修改 Hybrid A* 搜索算法、车辆参数、碰撞模型、地图缓存实现或手动 粗路径规划 的交互流程。
方案选择
采用“固定案例模板化并整体平移”的方案。
不采用只替换起终点的方案,因为地图和障碍仍停留在原始世界坐标,AMR 可能位于地图外。
不为每个案例分别编写实时构造逻辑,避免重复坐标换算、地图边界和障碍创建代码。
工厂接口与坐标规则
保留现有的 Create(CoarsePathTestScenario scenario),使离线和自动化测试仍可创建可重复的基准案例。
新增接受 AMR 世界位姿的工厂入口:
Create(CoarsePathTestScenario scenario,
double amrXMillimeters,
double amrYMillimeters,
double amrHeadingDegrees)
工厂将基准案例的起点映射到传入 AMR 位姿:
deltaX = amrX - baselineStartXdeltaY = amrY - baselineStartY- 起点位置为
amrX/amrY,起点航向为amrHeading。 - 目标位置、地图边界、圆形障碍、轴对齐矩形障碍和 TwoLeg 位置均加上
deltaX/deltaY。 - 目标航向为
amrHeading + (baselineGoalHeading - baselineStartHeading),再规范化到规划器采用的标准角度范围。 - TwoLeg 障碍自身的朝向保持基准值,只移动其位置;本变更是平移而非旋转场景几何。
所有固定案例都使用该规则,包含预期无解的障碍带案例。无解案例移动后仍必须保持无解。
UI 运行流程与错误处理
CoarsePathPlanningTestRunner.RunScenario 在开始时从 DetourInterface 读取一次 AMR 位姿,并将这份快照传给工厂。后续后台规划只使用冻结后的请求,定位更新不会改变正在执行的规划输入。
读取失败、空位姿或任一 X/Y/航向值为 NaN 或无穷值时,不提交规划请求。测试通过 Toast 和画布状态区显示 AMR 位姿不可用 及具体原因。
成功创建请求时,状态区额外显示冻结的 AMR 起点(世界 mm 和 deg),以便现场核对。既有的规划状态、路径搜索耗时、节点统计和失败原因显示继续保留。
手动 粗路径规划 测试不改变:它继续读取一次 AMR 起点,并要求用户输入终点、障碍和总超时。
缓存语义
缓存键继续由完整的地图输入决定。由于地图边界和障碍随 AMR 坐标平移,只有两次固定案例使用完全相同的 AMR 位姿时,粗路径规划-缓存命中 才会命中现有输入缓存。
AMR 已移动时显示缓存未命中是正确结果,而不是规划失败。UI 继续显示实际的缓存状态。
验证
自动化测试应验证:
- 新工厂入口将基准起点映射到实时 AMR 位姿,并正确保留目标相对位置与起终点航向差。
- 单矩形、多来源和无解案例的地图边界、目标及障碍按相同
deltaX/deltaY移动。 - TwoLeg 的位置移动而朝向不变。
- 既有
Create(scenario)的基准案例及其成功/无解预期继续通过。 - UI 运行器读取并冻结一次 AMR 位姿;无效位姿产生明确的未启动诊断。
- 文档准确说明实时 AMR 锚点、缓存命中条件和无解场景预期。
验收标准
- 任一固定 MovementTest 从当前 AMR 坐标和航向开始规划,而不是从写死起点开始。
- 目标、地图和障碍在当前 AMR 附近平移,起终点的相对方向要求保持不变。
- 现场定位异常时用户可直接看到原因,且不会错误进入规划后台任务。
- 离线固定案例和现有规划诊断能力不回归。