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.

## 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
all three layers. A current-session runtime fault also clears all three layers so stale active diagnostics are not left
on screen; an intentional bootstrap failure preserves its diagnostic world view. Stopping, running, or starting this
test must not issue chassis, motor, steering, brake, or gear output.