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, repeatedly plans EM trajectories, samples the
latest published trajectory, and draws the world, L-S, and T-S/T-V 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 direction segment `0` and waits for real direction confirmation; it does
not create or dispatch a direction-change action.

## 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 | 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. |
| `ReplanPeriodSeconds` | s | `0.20` | Minimum interval between EM planning cycles. |
| `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 | `2.0` | Single ST trajectory horizon, not the observer period. |
| `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 | `false` | Enables the 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 | `false` | Enables the optional native Painter fallback. |
| `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. |

The four vehicle fields are public editable MovementTest inputs. They, the map/timing fields, goal, and manual obstacles
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.
EM planning runs asynchronously with at most one planning cycle in flight. Every observer tick still captures fresh
state, samples the currently published trajectory, redraws all three 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 可以同时开启，也可全部关闭：全部关闭时仍保留 `OBSERVE_ONLY` 规划、控制台和 UI 状态。网页的启动、
快照或服务出现异常时会只记录一次中文诊断并熔断本次网页输出，不自动循环重启，也不会停止规划观察。

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

运动学页中 `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.

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.