Fix async Control/Plan toggle orphans and before-start waypoint index

[cursor]

Summary

Two high-severity correctness bugs found in recent async / trajectory paths:

1. Async Control/Plan toggle orphans and duplicates workers — Unchecking Control (or Planning) while AsyncThreadedExecuter was running treated the cooperative worker exit as a crash. step() cleared self.threads without stopping live peers, then vacuous all([]) restarted workers and left multiple Planner threads (Control could not come back cleanly).
2. update_waypoint_by_xy left a stale current_wp before path start — After driving far along a path and teleporting/resetting slightly upstream of the first waypoint (closest_wp==0, projected s < path_s[0]), neither update branch ran. Stanley used the far-path heading (often π/2 after an L-corner) → max steer; Pure Pursuit used the wrong target speed.

Root cause

• Stage flags were compared with is_alive() (call_control != alive), so intentional disable while the worker was still draining looked like a death; recreate wiped the join list without stop().
• Before-start Frenet s had no waypoint-update branch when the nearest index was already 0.

Fix

• Prune finished workers from the join list and _ensure_enabled_workers to start only stages that are enabled again (no peer teardown).
• create_threads keeps still-alive peers in the join list.
• update_waypoint_by_xy resets to current_wp=0 / next_wp=1 when ego is before path start.

Validation

PYTHONPATH=/workspace python3 -m pytest \
  test/c40_execution/test_c45_async_stage_toggle.py \
  test/c40_execution/test_c45_async_stop.py \
  test/c50_common/test_c54_trajectory_waypoint_update.py -v

11 passed.

Not re-reported (still open)

• PR Fix StopExecAtGoal async restart and combined stage order #22 — async stop restart / combined stage order
• PR Fix closed-loop Frenet wrap snap to s≈0 on last lap meters #24 — closed-loop Frenet wrap snap
• PR Fix Sync ZOH after Control off and Frenet SD orientation #26 — Sync ZOH after Control off / Frenet SD orientation