51 lines
3.1 KiB
Markdown
51 lines
3.1 KiB
Markdown
# 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.
|
|
|
|
## 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. |
|
|
|
|
The planning snapshot also uses the setup defaults: vehicle length `0.80 m`, vehicle width `0.60 m`, safety margin
|
|
`0.05 m`, and maximum curvature `1 / 1.20 m` (about `0.8333 1/m`). All fields and manual obstacles are read and frozen
|
|
before the background session starts. The live pose and actual longitudinal speed are then read once per observer tick.
|
|
|
|
## 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.
|