123 lines
7.5 KiB
Markdown
123 lines
7.5 KiB
Markdown
# EM 轨迹规划观察模式 MovementTest 设计
|
||
|
||
## 目标
|
||
|
||
在 `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest` 新增一个可在实车环境运行的
|
||
MovementTest。测试直接读取 MDCS 的实时定位、车速和车辆底盘状态，构造一段完整的
|
||
粗路径、平滑路径和 EM 轨迹规划流程，并持续展示规划结果。
|
||
|
||
该测试是**观察模式（observe only）**：它能完整生成、采样并打印“控制器此刻会使用的”
|
||
轨迹命令，但绝不向底盘、转向、电机或执行器写入任何运动命令。这让真实车辆保持由人工
|
||
或已有安全系统控制，同时可以验证定位坐标、地图、路径、速度曲线和时间参数是否一致。
|
||
|
||
## 使用方式与可配置项
|
||
|
||
MovementTest 对外提供以下可编辑字段，单位固定，避免毫米/米和角度/弧度混用：
|
||
|
||
| 字段 | 单位 | 默认值 | 含义 |
|
||
| --- | --- | --- | --- |
|
||
| `GoalXmm` | mm | 无 | 终点全局 X 坐标，启动前必须填写。 |
|
||
| `GoalYmm` | mm | 无 | 终点全局 Y 坐标，启动前必须填写。 |
|
||
| `GoalYawDeg` | deg | `0` | 终点航向角。 |
|
||
| `MapPaddingMeters` | m | `2.0` | 起点/终点矩形边界向四周的扩展距离。 |
|
||
| `MapResolutionMm` | mm | `50` | 局部占据栅格分辨率。 |
|
||
| `ReplanPeriodSeconds` | s | `0.20` | 使用最新车辆状态重新调用 EM 的周期。 |
|
||
| `ObserverPeriodSeconds` | s | `0.05` | 采样已发布轨迹、刷新状态信息的周期。 |
|
||
| `VehicleLengthMeters` / `VehicleWidthMeters` | m | 测试配置值 | 传给规划器的有效车辆外廓。 |
|
||
| `SafetyMarginMeters` | m | 测试配置值 | 车辆外廓之外的规划安全裕度。 |
|
||
| `MaximumCurvaturePerMeter` | 1/m | 测试配置值 | 实车允许的最大曲率。 |
|
||
|
||
启动后交互式输入障碍物，交互形式沿用现有粗路径 `MovementTest`：先输入数量，再逐个选择
|
||
圆形障碍物或轴对齐矩形障碍物，并输入其中心、尺寸或半径。输入坐标使用全局 mm；障碍物
|
||
被转换到该次规划的局部地图坐标。
|
||
|
||
输入校验必须拒绝非有限值、非正的地图扩展/分辨率/周期、无效车辆尺寸和无效曲率。若起点
|
||
与终点无法形成容纳车辆和安全裕度的有效区域，也必须打印原因并不启动规划。
|
||
|
||
## 车辆数据与坐标约定
|
||
|
||
每次启动和重规划均从 `MDCS_basictool` 读取：
|
||
|
||
- `DetourInterface.getCartLocation()`：车辆实时全局位置与航向；其 mm/deg 数据统一转换为
|
||
`Pose2D` 所使用的 m/rad。
|
||
- `BasicPilotBase.Chassis.GetCarSpeed(true)`（或该项目等价的实际底盘读数）：车辆实际车体
|
||
纵向速度，用于 `MotionState.SignedLongitudinalVelocity` 和日志。
|
||
- 底盘对象：只用于读取可用性与实际状态；车辆外廓、裕度和曲率限制由测试显式配置，
|
||
因为轮组几何本身不能可靠推导车身外形和业务安全距离。
|
||
|
||
EM 的 `VelocityX`/`VelocityY` 是世界坐标速度，不可直接传给 MDCS 的车体坐标
|
||
`SendXYThSpeed(vx, vy, vth)`。本测试也不会调用该接口。若以后单独实现执行模式，非完整横移
|
||
模型应使用 `SignedLongitudinalVelocity` 作为车体 `vx`、零车体 `vy`，并把 `YawRate` 从 rad/s
|
||
转换为 MDCS 所需的 deg/s；执行模式还必须经过独立的安全评审。
|
||
|
||
## 地图、路径与规划流程
|
||
|
||
1. 读取当前定位作为本次测试的起点，读取配置的终点。
|
||
2. 以起点和终点为对角线构造轴对齐矩形，四个方向均扩展 `MapPaddingMeters`；将障碍物栅格化
|
||
到此局部地图。
|
||
3. 用现有 Hybrid A* 生成粗路径，再以现有 Local G2 平滑器生成平滑参考路径。任何一个上游
|
||
环节失败时打印状态、保留世界视图和失败原因，不进入 EM 循环。
|
||
4. 从平滑路径建立 `ReferencePathSegmenter`。首次成功后创建
|
||
`EmPlanningCoordinator`、`EmPlanningService` 和 `TrajectoryExecutor`。
|
||
5. 每个 `ReplanPeriodSeconds` 读取最新车辆状态，以同一地图、参考路径和终点构造
|
||
`EmPlanningRequest`。协调器在上一条有效轨迹基础上滚动发布下一条 `EmTrajectory`；新的
|
||
失败不会覆盖上一条已经验证成功的轨迹。
|
||
6. 每个 `ObserverPeriodSeconds` 按 `EmTrajectory.Metadata.EffectiveAtUtc` 和每个点的
|
||
`TimeFromStart` 采样当前时刻的目标点，并调用 `TrajectoryExecutor.UpdateCommand` 得到
|
||
仅供显示的 `TrajectoryControlCommand`。
|
||
|
||
`EmTrajectoryPoint` 已携带 `TimeFromStart`、`SignedLongitudinalVelocity` 与 `YawRate`，因此不
|
||
需要外部猜测轨迹时间，也不应通过相邻 yaw 差分计算角速度。
|
||
|
||
初版只观察当前方向段。若轨迹到达 `GearSwitch`，界面和终端会明确显示“等待真实档位/方向
|
||
确认”，不会假设车辆已经换向，也不会自动推进到下一方向段。这样可避免静止观察测试在倒车
|
||
段产生虚假的完成状态。
|
||
|
||
## UI 与终端输出
|
||
|
||
测试在生命周期内维护三组独立 painter；停止、取消或异常时清除对应图层。
|
||
|
||
| 图层 | 内容 |
|
||
| --- | --- |
|
||
| `TrajectoryObserver.World` | 局部地图边界、障碍物、实时车辆位姿、起点、终点、Hybrid A* 粗路径、G2 平滑路径和最新 EM 轨迹。 |
|
||
| `TrajectoryObserver.LS` | 将最新 EM 点投影到当前 `DirectionSegmentView`，绘制参考弧长 `s` 与横向偏移 `l` 的曲线，附上投影失败数。 |
|
||
| `TrajectoryObserver.ST` | 绘制 `t-s` 曲线，并在同一图层标注或以第二条曲线展示 `t-v`，用于检查时间推进、纵向进度和速度。 |
|
||
|
||
LS/ST 由最终 `EmTrajectory` 派生，不修改当前 EM 的公共结果接口：LS 的 `s/l` 来自
|
||
`FrenetProjector.TryProject`，ST 的 `t` 来自 `TimeFromStart`、`s` 来自 `PathS`，速度来自
|
||
`SignedLongitudinalVelocity`。这保证展示数据正是控制器将要消费的发布轨迹。
|
||
|
||
终端启动时打印起终点、地图尺寸、配置和障碍物摘要；上游每步打印成功/失败和耗时；运行中按
|
||
节流周期打印规划序号、定位、实际纵向速度、轨迹状态、选中点、目标速度、目标 yaw-rate、
|
||
LS 投影统计以及重规划耗时。每一段运行日志都带有：
|
||
|
||
`OBSERVE_ONLY: no chassis command is sent.`
|
||
|
||
## 明确的安全边界
|
||
|
||
该 MovementTest 中禁止调用下列或等价的硬件写接口：
|
||
|
||
- `MultiWheelChassis.SendXYThSpeed`
|
||
- `MultiWheelChassis.SendMotion`
|
||
- 任何轮速、轮角、制动释放或执行器 adapter 写接口
|
||
- 任何会启动现有闭环几何控制器并输出到底盘的接口
|
||
|
||
`TrajectoryControlCommand` 仅用于控制器适配验证、屏幕展示和终端打印。运动状态不正常、定位
|
||
不可用、地图外、规划失败或取消时，测试仅停止/保留观察信息并报错；不尝试“补救性”控制车辆。
|
||
|
||
## 验证标准
|
||
|
||
实现完成后至少验证：
|
||
|
||
1. 项目可编译，测试类可以被现有 TestRunner 发现和启动。
|
||
2. 在不连接可写底盘的环境中，以可复现的定位/地图数据运行一次，确认粗路径、平滑路径和
|
||
EM 轨迹均可显示。
|
||
3. 每条发布轨迹的 `TimeFromStart` 单调、ST 图与 `PathS` 一致，LS 图能报告可投影点和失败点。
|
||
4. 源码检查与运行日志都证明观察循环未触发任一底盘写方法。
|
||
5. 取消测试后后台循环退出，三个 painter 图层被清理，不残留定时任务。
|
||
|
||
## 非目标
|
||
|
||
本次不实现实车自动驾驶、档位切换、底盘写命令、安全冗余、急停策略或把 EM 轨迹转换为旧的
|
||
`AbstractTrack` 几何控制器输入。这些属于后续独立、可评审的执行模式工作。
|