Files
ParkingRobot/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest

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.

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. Stopping, running, or starting this test must not issue chassis, motor, steering, brake, or gear output.