ParkingRobot/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md

# EM trajectory observation MovementTest

`TrajectoryObservationMovementTest` is an observe-only host for the real MDCS localization and chassis-speed read
interfaces. It bootstraps the coarse path and Local G2 reference once, requests one frozen full-direction-segment EM
plan per active direction segment by default, samples the latest published trajectory, and draws the world, LS, and ST
layers. It does not drive, steer, brake, change gear, or invoke a geometric vehicle controller.

Every runtime status contains `OBSERVE_ONLY: no chassis command is sent.` Treat the displayed control command as a
diagnostic prediction only. Goal and rolling-safety-stop commands are logged, never applied to hardware. At the end of a
gear-switch trajectory, the observer remains on the current direction segment and waits for real direction confirmation;
it does not create or dispatch a direction-change action.

Every frozen observation session writes a text report to
`TrajectoryObservationReports/trajectory-observation-session-<id>.txt` in the process working directory when the
session stops; the report path is printed to the console.

## FullDirectionSegment observation (default)

`UseFullDirectionSegmentPlanning=true` maps to `EmPlanningScope.FullDirectionSegment`. MovementTest performs one frozen
optimization for the complete remaining active direction segment, then republishes that trajectory with fresh observation
state until the real direction boundary is confirmed and the next segment becomes active. `s_end` is the actual Local G2
`PathS` at the active segment boundary; `T_end` is derived from the feasible acceleration, cruise, jerk-limited stopping,
and zero-speed hold behavior instead of being forced by a legacy time horizon.

`DistanceHorizonMeters` and `TimeHorizonSeconds` are rolling-compatible fields and do not truncate full mode. Forward
desired/hard maximum speed is `1.0 m/s`; reverse desired/hard maximum speed is `0.5 m/s`. A successful real boundary
requires a terminal stop plus exact world-position and normalized yaw matching (`0 m` / `0 rad`).

## Rolling trajectory observation

Rolling trajectory observation remains `OBSERVE_ONLY` and never sends a chassis command. Rolling and approach trajectories
may end with nonzero speed because `DistanceHorizonMeters` is an L-S reference window and `TimeHorizonSeconds` is the
single S-T output duration. Only a real Goal or gear-switch boundary may publish an exact zero-speed terminal.

## Configuration

| Field | Unit | MovementTest UI default | Meaning |
| --- | --- | ---: | --- |
| `GoalXmm` | world mm | `NaN` | Goal X. If X or Y is not finite, the host prompts for X, Y, and yaw. |
| `GoalYmm` | world mm | `NaN` | Goal Y. |
| `GoalYawDeg` | world deg | `0` | Goal heading. |
| `MapPaddingMeters` | m | `2.0` | Padding added on all sides of the start/goal bounds. |
| `MapResolutionMm` | mm | `50` | Local occupancy-grid resolution. |
| `UseFullDirectionSegmentPlanning` | bool | `true` | Selects `FullDirectionSegment`; set `false` for legacy `RollingHorizon`. |
| `ReplanPeriodSeconds` | s | `0.20` | Minimum interval between EM planning cycles in rolling scope. |
| `ObserverPeriodSeconds` | s | `0.05` | Live-state sampling and redraw interval. |
| `SolverTimeoutSeconds` | s | `5.0` | EM solver timeout frozen for the session. |
| `MaximumOsqpIterations` | iterations | `100000` | OSQP iteration limit frozen for the session. |
| `TimeHorizonSeconds` | s | `20.0` | Rolling-only ST trajectory horizon, not the observer period; full mode derives `T_end` instead. |
| `OutputTimeStepSeconds` | s | `0.10` | Published trajectory timestamp spacing. |
| `VehicleLengthMeters` | m | `0.80` | Vehicle envelope length supplied to coarse, smoothing, and EM planning. |
| `VehicleWidthMeters` | m | `0.60` | Vehicle envelope width supplied to planning. |
| `SafetyMarginMeters` | m | `0.05` | Additional planning clearance outside the vehicle envelope. |
| `MaximumCurvaturePerMeter` | 1/m | `1 / 1.20` | Maximum allowed vehicle curvature (about `0.8333 1/m`). |
| `EnableWebVisualization` | bool | `true` | Enables the primary local web dashboard session. |
| `AutoOpenWebVisualization` | bool | `true` | Opens the tokenized local dashboard URI when web output is enabled. |
| `WebVisualizationPort` | port | `0` | `0` selects an ephemeral port; otherwise use `1024` through `65535`. |
| `WebRefreshRateHz` | Hz | `10` | Maximum dynamic web snapshot publication rate. |
| `VisualizationHistoryCycleLimit` | cycles | `60` | Bounded web history length. |
| `EnableNativePainterVisualization` | bool | `true` | Enables optional native Painter audit windows; disable unless explicitly reviewed. |
| `DirectionConfirmationSpeedMetersPerSecond` | m/s | `0.02` | Minimum signed-speed magnitude used to confirm the next direction. |
| `DirectionConfirmationSamples` | samples | `3` | Strictly increasing matching-direction samples required after the stop hold. |
| `GearSwitchProjectionToleranceMeters` | m | `0.50` | Maximum switch-point projection distance for both adjacent segments. |
| `GearSwitchStopHoldSeconds` | s | `0.20` | Continuous real-stop duration required before direction samples count. |

`TrajectoryObservationSettings` contract defaults are `PlanningScope=FullDirectionSegment`,
`TimeHorizonSeconds=2.0`, `EnableWebVisualization=true`, and `EnableNativePainterVisualization=false`; the MovementTest
UI currently exposes the values above. All public fields are validated and copied into a frozen input snapshot before
the background session starts. Later edits cannot change an active session. The live pose and actual longitudinal speed
are then read once per observer tick.

In full scope, EM planning runs once per active segment with one attempt in flight. In rolling scope, EM planning
respects `ReplanPeriodSeconds` with at most one planning cycle in flight. Every observer tick still captures fresh
state, samples the currently published trajectory, redraws all layers, and emits a session-guarded status; a slow
planner therefore does not reduce the configured observation cadence.

## Manual obstacles

Enter a count from `0` through `20`, then select each obstacle type. Coordinates are global/world millimeters.

- Circle example: center `(2500, 1200) mm`, radius `300 mm`.
- Axis-aligned rectangle example: center `(4000, -500) mm`, X length `800 mm`, Y width `500 mm`.

The complete obstacle envelope must fit inside the start/goal bounds plus `MapPaddingMeters`; otherwise bootstrap is
rejected before EM planning.

## Reading the layers

- `TrajectoryObserver.World` shows map bounds and occupied cells, the frozen start and goal, Hybrid A* coarse path,
  Local G2 smoothed path, current real pose, and latest published EM trajectory.
- `TrajectoryObserver.LS` plots reference path-S horizontally and lateral offset vertically. Projection failures indicate
  trajectory points that could not be associated with the current direction segment.
- `TrajectoryObserver.ST` overlays time-to-path-S and time-to-signed-speed. Use it to check monotonic time/progress,
  stop profiles, and the sign of forward/reverse velocity.

`EmTrajectoryPoint.VelocityX` and `VelocityY` are world-frame components. They are not chassis-frame velocity commands
and must never be forwarded directly to a vehicle motion interface. This MovementTest performs no coordinate conversion
for driving and has no driving capability; any future execution mode requires a separate safety-reviewed design.

## 网页看板与可选 Painter

将 `EnableWebVisualization=true` 可启用本机网页看板；它只绑定 `127.0.0.1`，启动日志会给出完整的、带随机
会话令牌的 URL。端口为 `0` 时由系统选择可用回环端口；浏览器必须使用该完整 URL，不能删掉 token。默认
`AutoOpenWebVisualization=true` 会尝试打开默认浏览器；无桌面会话或打开失败只记录 URL，服务和观察循环继续运行。
网页默认以 `10 Hz` 发布不可变动态快照，网页关闭、断开或慢客户端只会丢弃网页帧，绝不会阻塞、序列化等待或改变
规划周期。

`EnableNativePainterVisualization=true` 才会为本会话创建 `TrajectoryObserver.World`、`LS`、`ST` Painter。网页是主
观察界面，Painter 仅作为可选的正确性/审计输出；受监督车辆清单执行前应显式关闭 Painter，除非另行审查。网页与
Painter 可以同时开启，也可全部关闭：全部关闭时仍保留 `OBSERVE_ONLY` 规划、控制台和 UI 状态。网页的启动、快照或
服务出现异常时会只记录一次中文诊断并熔断本次网页输出，不自动循环重启，也不会停止规划观察。

网页采用中文说明配合科研绘图约定：白底、细灰网格和细曲线；当前轨迹为蓝色实线，上一轮为灰色虚线，换向与交接点
为橙色，红色仅表示实际越界或失败。路径总览把已完成方向段画为灰色细实线、活动段画为蓝色细实线、未来段画为浅灰
虚线，并以深蓝覆盖当前规划视界。页面的 `ReferenceS (m)` 是将轨迹投影到**完整活动方向段**后的共享参考站；
`PathS (m)` 是 ST 求解使用、从该段局部起点累计的实际路径弧长，二者不可互相替代或跨段直接相减。

所有图表轴标签与单位由实际快照提供：世界 `X/Y (m)`、`ReferenceS (m)`、`PathS (m)`、`t (s)`、`l (m)`、
`v (m/s)`、`a (m/s²)`、`j (m/s³)`、`κ (m⁻¹)`、`ω (rad/s)`。`ls` 的横轴是 `ReferenceS (m)`、纵轴是 `l (m)`；
`st` 的横轴是 `t (s)`、纵轴是 `PathS (m)`。每张图支持鼠标滚轮以指针为中心缩放、拖拽框选局部放大、`重置视图`
与 `全屏`；这些操作只改变本地视口，不修改原始快照，也不向规划器发送参数。

运动学页中 `j[i]` 仅表示真实区间 `[tᵢ, tᵢ₊₁)`，所以 jerk 只有 `N-1` 个样本；末点之后没有虚构的 `j=0`。
状态页将滚动续航、接近停车边界和精确停车边界分别标为 `RollingContinuation`、
`ApproachStopBoundary`、`ExactStopAtBoundary`：前两者允许非零末端速度，只有真实边界才是精确停车锚点。换向高亮
仅会在投影位置满足容差、已连续停车保持 `GearSwitchStopHoldSeconds`，并取得
`DirectionConfirmationSamples` 个满足 `DirectionConfirmationSpeedMetersPerSecond` 阈值的下一方向带符号速度样本后，
严格从 `N -> N+1` 前进。

## Operator launch and stop checklist

The vehicle UI entry is `EM轨迹规划观察闭环测试`. It is an observation session only; it has no chassis,
motor, steering, brake, or gear output.

Before starting:

1. Ensure the vehicle is in a safe, supervised state; this test reads live localization and signed chassis speed but
   never controls the vehicle.
2. Confirm localization and the chassis read interface are available. The session reports a startup failure if either
   cannot be read.
3. Set a finite goal X/Y/yaw (or be ready to enter them when prompted), then enter zero to twenty manual obstacles.
   Each obstacle must remain inside the configured start/goal rectangle plus map padding.
4. Keep `EnableWebVisualization=true` and set `EnableNativePainterVisualization=false` unless the Painter audit output is
   explicitly required and reviewed for this vehicle session.

To start, select the entry in the vehicle UI and start the MovementTest. Confirm the status begins with
`OBSERVE_ONLY: no chassis command is sent.`, then review the bootstrap status and the
`TrajectoryObserver.World`, `TrajectoryObserver.LS`, and `TrajectoryObserver.ST` layers. A logged trajectory control
command is diagnostic information only and must not be copied into a vehicle-control interface.

To stop, use the vehicle UI's normal MovementTest stop action. Confirm the status says
`Observation stop requested; all observer layers were cleared.` The cancellation request stops observer work and clears
an existing Painter, stops the web publisher/HTTP/SSE service, and releases its port. A current-session runtime fault
also clears an existing Painter and stops the web session so stale diagnostics are not left on screen; an intentional
bootstrap failure still reports through UI/console even when both visualization modes are off. Closing a browser tab does
not stop the session; `TestStop()` owns final web and Painter cleanup. Stopping, running, or starting this test must not
issue chassis, motor, steering, brake, or gear output.