Fix VelocityLocalPlanner end-of-path crash and Control Step ego desync

[cursor]

Bug and impact

1. VelocityLocalPlanner.replan crashes at the final global waypoint — slice_trajectory_horizon builds a 1-point TrajectoryTracker; Frenet conversion indexed next_wp=1 and raised IndexError. Hits SAN Campus / any stack using VelocityLocalPlanner (including lattice velocity stage) when the ego reaches path end.
2. Control Step / Steer leave stack ego stale after the world/stack ego split — plant moves, UI/pm.ego_vehicle stay put until the next GT tick (same class of bug as the Align fix in Fix Control Align ego split regression and TrajectoryTracker final-waypoint crash #16).
3. Save Start captured live velocity into the reset snapshot, so Reset could restore a non-zero speed even though the profile YAML only stores [x,y,theta].

Root cause

• convert_xy_path_to_sd_path / _np assumed closest_wp==0 ⇒ next_wp=1 without checking path length.
• Manual Control UI called world.control_ego_state without syncing stack PM.
• State.set_start() copies all fields including velocity; Save Start did not zero velocity for the snapshot.

Fix

• Clamp Frenet segment indices for 1-point paths; safe-handle zero-length normals.
• Add VisualizerApp.apply_world_control (plant + stack dual-write) and route Control Step/Steer/Accel through it.
• Save Start snapshots velocity 0 while preserving live speed.

Validation

PYTHONPATH=/workspace python3 -m pytest \
  test/c50_common/test_c54_trajectory_horizon_slice.py \
  test/c20_planning/test_c27_velocity_planner_end_of_path.py \
  test/c40_execution/test_c44_pm_ego_pose.py \
  test/c10_perception/test_c11_save_start_velocity.py \
  test/c50_common/test_c54_trajectory_waypoint_update.py -v

20 passed.

  

[cursor]

Bugbot is not enabled for your account, so this pull request was not reviewed.

Enable Bugbot in the Cursor dashboard to get automatic reviews on future PRs.