# EM 轨迹规划 MovementTest（观察与闭环）

`tarjplanner_movementtest` 将粗路径、Local G2 连续参考和 EM 轨迹规划接入两个 MovementTest：

- `EM轨迹规划观察闭环测试`：只读地采样真实定位和底盘速度，规划并观察轨迹，**绝不向底盘发送命令**。
- `EM闭环测试`：冻结首个 EM 方向段后，把轨迹交给现有几何控制器执行；这是会实际控制车辆的测试，必须在受监督的安全场地使用。

两种测试都会在启动时冻结目标、手动障碍物、地图和规划配置。它们不读取或修改传感器地图，也不做持续重建图。观察测试的所有状态都包含 `OBSERVE_ONLY: no chassis command is sent.`；其中显示的控制命令只是诊断预测，不能转发到硬件。

## 模块说明（Module Overview）

| 模块 | 负责内容 | 不负责内容 |
| --- | --- | --- |
| `Map` | 根据冻结的起点、终点和手动障碍物创建不可变规划地图 | 实时环境更新、车辆控制 |
| `CoarsePath` | Hybrid A* 粗路径、碰撞复核与方向分段 | 速度规划、底盘命令 |
| `PathSmoothing` | 将粗路径转换为连续的 Local G2 参考 | 实时状态读取、控制 |
| `EMPlanner` | 针对活动方向段生成不可变 `EmTrajectory` | 定位读取、UI、硬件协议 |
| `TrajectoryExecution` | 轨迹采样、换向状态和诊断性控制意图 | 底盘接口、命令下发 |
| 本目录 | 会话编排、状态采样、可视化、报告，以及闭环测试的轨迹适配 | 改写上游规划结果或地图快照 |

`EM轨迹规划观察闭环测试` 在换向边界只等待真实方向确认；不创建、不派发换向动作。`EM闭环测试` 只执行冻结的首个方向段；若终端是换向边界，它停车后不会自动启动下一段。

## 文件结构（File Structure）

```text
tarjplanner_movementtest/
├── README.md                                      # 模块边界、配置、操作和限制
├── MovementTest.TrajectoryObservationTest.cs      # 只读观察 UI 入口与会话生命周期
├── MovementTest.EmClosedLoopTest.cs               # 实际闭环 UI 入口、舵轮前向校验与停止
├── EmControlTrajectoryAdapter.cs                  # EM 单方向段轨迹转几何控制器 Trajectory2D
├── TrajectoryObservationContracts.cs              # 设置快照、障碍物、启动输入和结果契约
├── TrajectoryObservationPipeline.cs               # Bootstrap、EM 周期、采样和观察循环
├── TrajectoryObservationPlanningDeadline.cs       # 单周期截止时间与发布授权
├── TrajectoryObservationSegmentTracker.cs         # 终停、真实换向确认和活动方向段推进
├── TrajectoryObservationStaticSnapshotBuilder.cs  # 地图、路径、配置的静态网页快照
├── TrajectoryObservationDynamicSnapshotBuilder.cs # 每个 tick 的动态 World/LS/ST 快照
├── TrajectoryObservationKinematicChartBuilder.cs  # 速度、加速度、jerk、曲率等图表数据
├── TrajectoryObservationHandoffAnalyzer.cs        # 轨迹交接与连续性诊断
├── TrajectoryObservationDiagnostics.cs            # 配置、规划和运行诊断模型
├── TrajectoryObservationPresentation.cs           # Painter 图层与中文状态文本
├── TrajectoryObservationVisualizationPublisher.cs # 本机网页会话与 Painter 发布隔离
└── TrajectoryObservationReportWriter.cs           # 只读观察会话文本报告
```

## 运行数据流（Runtime Data Flow）

```text
目标、手动障碍物、实时起始位姿和冻结设置
                  │
                  ▼
        Map -> CoarsePath -> PathSmoothing
                  │
                  ▼
      当前方向段的 EMPlanner -> EmTrajectory
                  │
        ┌─────────┴──────────────────┐
        ▼                            ▼
只读观察：每个 tick 读取状态      闭环测试：首段 Trajectory2D
并采样/绘制/报告                  -> 现有几何控制器 -> 底盘
```

观察测试中，规划失败、报告写入失败、网页或 Painter 发布失败均只产生诊断，不能改变其只读属性。闭环测试在控制器接管前检查底盘类型和四个舵轮是否与车体前向平行；不满足时不会接管底盘。

## 运行状态与停止（Runtime State and Stop）

每个测试同时最多保留一个活动会话。重新启动会取消前一会话；`TestStop()` 取消规划和后台任务，并清理 Painter 与网页发布器。

观察测试在停止时把会话内容写入进程工作目录的：

```text
TrajectoryObservationReports/trajectory-observation-session-<id>.txt
```

报告路径会输出到控制台。关闭浏览器标签不会停止会话；必须使用 MovementTest 的正常停止操作。闭环测试停止时会请求停止当前 `DriveTask`，并清理其可视化资源。

## 坐标与单位（Coordinates and Units）

| 项目 | 坐标/单位 | 说明 |
| --- | --- | --- |
| UI 目标与手动障碍物 | 世界坐标 `mm`、航向 `deg` | 入口处转换为规划用 SI 单位。 |
| `Pose2D` / EM 路径 | `m`、`rad` | 车辆几何中心的世界位姿。 |
| `ReferenceS` | `m` | 投影到完整活动方向段后的共享参考站。 |
| `PathS` | `m` | ST 求解使用、从活动段局部起点累计的弧长；不可与 `ReferenceS` 跨段直接相减。 |
| `l`、`v`、`a`、`j`、`κ`、`ω` | `m`、`m/s`、`m/s²`、`m/s³`、`m⁻¹`、`rad/s` | 网页/Painter 图表使用的横纵轴单位。 |

`EmTrajectoryPoint.VelocityX` 与 `VelocityY` 是世界坐标系分量。观察测试不会进行驱动坐标转换，也不会把它们发送到车辆接口。闭环测试使用 `EmControlTrajectoryAdapter`，以世界位置重建弧长并保留有符号纵向速度、航向和曲率，得到现有控制器使用的 `Trajectory2D`。

## 最小使用说明（Minimal Use）

### 只读观察

1. 在车辆 UI 选择 `EM轨迹规划观察闭环测试`。
2. 设置有限的 `GoalXmm`、`GoalYmm`、`GoalYawDeg`；X 或 Y 为 `NaN` 时会依次弹出输入框。
3. 输入 `0` 至 `20` 个手动障碍物，或输入 `0` 使用空障碍物快照。
4. 启动测试，确认状态含有 `OBSERVE_ONLY: no chassis command is sent.`。
5. 查看网页或 Painter 的 World、LS、ST 与运动学图层；停止时使用 UI 的正常停止操作。

### 实车闭环

1. 仅在已审查、受监督的车辆和场地中选择 `EM闭环测试`。
2. 确认实时定位与底盘读接口可用；确认底盘为 `MultiWheelChassis`，且四个舵轮已对齐车头方向。
3. 设置目标、障碍物以及命令速度、偏离阈值和执行超时。
4. 启动后先等待“规划完成”状态；只有通过舵轮校验后，冻结的首个方向段才会交给现有控制器。
5. 任意异常或人工停止时，使用 `TestStop()` 所对应的 UI 停止动作；确认控制任务已停止。

## 配置（Configuration）

### 两个入口共有的规划与可视化配置

| 字段 | 单位 | UI 默认值 | 含义 |
| --- | --- | ---: | --- |
| `GoalXmm` / `GoalYmm` | 世界 mm | `NaN` | 终点位置；非有限值时弹出输入。 |
| `GoalYawDeg` | 世界 deg | `0` | 终点航向。 |
| `MapPaddingMeters` | m | `2.0` | 起点/终点范围四周增加的地图边距。 |
| `MapResolutionMm` | mm | `50` | 局部占据栅格分辨率。 |
| `SolverTimeoutSeconds` | s | `5.0` | 冻结的单次规划截止时间。 |
| `MaximumOsqpIterations` | 次 | `100000` | OSQP 最大迭代数。 |
| `OutputTimeStepSeconds` | s | `0.10` | 发布轨迹相邻时间戳间隔。 |
| `VehicleLengthMeters` / `VehicleWidthMeters` | m | `0.80` / `0.60` | 规划使用的车辆外形尺寸。 |
| `SafetyMarginMeters` | m | `0.05` | 车辆外形之外的额外安全余量。 |
| `MaximumCurvaturePerMeter` | `m⁻¹` | `1 / 1.20` | 允许的最大曲率。 |
| `EnableWebVisualization` | bool | `true` | 启用本机网页看板。 |
| `AutoOpenWebVisualization` | bool | `true` | 网页会话启动后尝试打开浏览器。 |
| `WebVisualizationPort` | port | `0` | `0` 自动选择回环端口；否则必须为 `1024` 至 `65535`。 |
| `WebRefreshRateHz` | Hz | `10` | 动态网页快照的最高发布频率。 |
| `VisualizationHistoryCycleLimit` | cycles | `60` | 网页保存的有限历史周期数。 |
| `EnableNativePainterVisualization` | bool | `true` | 启用可选 Painter 图层。 |

`TrajectoryObservationSettings` 的契约默认值与 UI 不完全相同：`TimeHorizonSeconds=2.0`、`EnableNativePainterVisualization=false`，其余入口字段由各 MovementTest UI 显式赋值。启动前调用 `CreateValidatedSnapshot()`；之后 UI 改动不会影响活动会话。

### 只读观察专用配置

| 字段 | 单位 | UI 默认值 | 含义 |
| --- | --- | ---: | --- |
| `UseFullDirectionSegmentPlanning` | bool | `true` | `true` 使用 `FullDirectionSegment`，`false` 使用 `RollingHorizon`。 |
| `ReplanPeriodSeconds` | s | `0.20` | 滚动模式两次规划之间的最小间隔。 |
| `ObserverPeriodSeconds` | s | `0.05` | 读取状态、采样、绘图的观察周期。 |
| `TimeHorizonSeconds` | s | `20.0` | 仅滚动兼容字段；完整方向段模式不用于截断。 |
| `DirectionConfirmationSpeedMetersPerSecond` | m/s | `0.02` | 换向确认所需的下一方向速度阈值。 |
| `DirectionConfirmationSamples` | samples | `3` | 停车后连续有效方向样本数。 |
| `GearSwitchProjectionToleranceMeters` | m | `0.50` | 换向点在相邻两段上的最大投影距离。 |
| `GearSwitchStopHoldSeconds` | s | `0.20` | 方向样本开始计数前要求的连续停车保持时间。 |

完整方向段模式对每个活动方向段仅规划一次；滚动模式遵守 `ReplanPeriodSeconds`。无论规划是否较慢，观察循环仍按 `ObserverPeriodSeconds` 读取新鲜状态并发布当前轨迹快照。

### 闭环专用配置

| 字段 | 单位 | UI 默认值 | 含义 |
| --- | --- | ---: | --- |
| `MaximumCommandSpeedMetersPerSecond` | m/s | `1.00` | 交给现有控制器的最大命令速度。 |
| `MaximumDistanceToTrajectoryMeters` | m | `0.30` | 控制器允许的最大轨迹偏离距离。 |
| `ExecutionTimeoutSeconds` | s | `120` | 首个方向段执行超时。 |
| `WheelAlignmentToleranceDegrees` | deg | `2` | 四个舵轮与前向平行的最大角度误差。 |

闭环入口固定使用 `FullDirectionSegment`，规划成功后只执行第一个活动方向段。

## 手动障碍物与会话冻结（Obstacles and Frozen Session）

两个入口都要求输入 `0` 至 `20` 个障碍物，坐标均为世界 `mm`：

- 圆形：中心 `(2500, 1200) mm`、半径 `300 mm`。
- 轴对齐矩形：中心 `(4000, -500) mm`、X 向长度 `800 mm`、Y 向宽度 `500 mm`。

障碍物完整包络必须位于起点/终点矩形加 `MapPaddingMeters` 后的地图范围内，否则启动阶段拒绝会话。启动成功后，目标、障碍物、地图、车辆参数和配置均为会话快照；不会跟随 UI 后续编辑改变。

## 网页看板与可选 Painter（Visualization）

启用 `EnableWebVisualization=true` 后，服务只绑定 `127.0.0.1`。启动日志输出含随机会话令牌的完整 URL；必须使用完整地址，不能去掉 token。端口为 `0` 时系统选择可用回环端口。浏览器打开失败、客户端断开或快照发布异常只记录诊断并隔离该网页输出，不应阻塞规划、观察或闭环停止。

启用 `EnableNativePainterVisualization=true` 后，可查看 `TrajectoryObserver.World`、`TrajectoryObserver.LS` 与 `TrajectoryObserver.ST` 图层。网页是主观察界面；Painter 是可选审计输出。网页与 Painter 可同时启用，也可同时关闭。

- World：地图边界、占据格、冻结起点/终点、粗路径、Local G2、实时位姿和 EM 轨迹。
- LS：横轴 `ReferenceS (m)`，纵轴 `l (m)`；投影失败表示点无法关联到活动方向段。
- ST：横轴 `t (s)`，纵轴 `PathS (m)`；用于检查时间、进度、停车与速度符号。
- 运动学图：显示 `v`、`a`、`j`、`κ`、`ω` 等快照数据。`j[i]` 对应区间 `[tᵢ, tᵢ₊₁)`，因此只有 `N-1` 个样本。

## 报告与诊断（Reports and Diagnostics）

只读观察会话把带时间戳的诊断写入文本报告，包含会话编号、运行状态和 `OBSERVE_ONLY` 安全标识。常见状态包括当前活动方向段、规划是否已发布、轨迹采样结果、换向确认进度和可视化地址。

闭环测试会在控制器接管前输出冻结轨迹点数、控制点数、长度、方向和终端类型；执行期间输出底盘速度与前后 GCP 角度，供监督人员诊断。该测试的命令输出属于真实控制流程，不能按观察模式解释。

## 常见错误（Common Errors）

| 现象 | 检查项 |
| --- | --- |
| 启动即提示定位或底盘接口不可用 | 确认 MDCS 定位和底盘只读接口已连接；观察与闭环入口都需要初始状态。 |
| 目标或障碍物输入被拒绝 | 检查所有值为有限数；障碍物数量为 `0–20`；半径/边长为正；包络位于地图范围内。 |
| 设置校验失败 | 正数配置不得为零或非有限；`OutputTimeStepSeconds` 不得大于 `TimeHorizonSeconds`；端口只能为 `0` 或 `1024–65535`。 |
| 观察测试没有车辆动作 | 这是预期行为。该入口永远是 `OBSERVE_ONLY`。 |
| 闭环测试未接管车辆 | 检查首段规划是否成功、控制轨迹是否至少有两个不同位置点、底盘类型以及舵轮前向对齐。 |
| 闭环在换向边界停止 | 这是预期行为；该测试不自动执行下一方向段。 |
| 网页没有打开或无法访问 | 从控制台复制完整令牌 URL；确认使用本机回环地址；打开失败不会停止会话。 |

## 当前限制（Current Limits）

- 地图和障碍物只在启动时冻结；本模块不实现实时感知更新或全局重规划。
- 观察测试不具备任何底盘、转向、制动或换向命令能力。
- 闭环测试只覆盖冻结的首个方向段，换向后续段不自动执行。
- 闭环控制依赖现有 `TrajectoryTrackingMovement` 和 `MultiWheelChassis`；它不是对任意底盘的通用执行接口。
- 网页服务仅限本机回环访问，且可视化故障只作为诊断处理。