Repository navigation
feat(route): multi-waypoint route on the map - #4979
dmrpereira wants to merge 5 commits into
Conversation
Route and Waypoint state machine (data-model rules), the route_cmd and route_state documents, and the Rerun overlay converters for waypoints and the route line.
RouteFollower interposes on map clicks, feeds the planner one goal at a time with pass-through waypoints, and detects completion, failure, stop and outside interruption. RouteControl is the pygame Route window with a heartbeat keepalive.
Composes the Go2 smart stack with RouteFollower and RouteControl, remaps the shared streams, and merges the route overlays into the Go2 rerun config. Adds pygame to the unitree extra and the registry entries.
Spec, plan, research, data model, stream contract, quickstart and task list for the multi-waypoint route feature.
github-actions
Bot
added
the
first-time-contributor
 [Medium impact] The PR adds an opt-in route workflow for Go2 navigation. Do not merge until the waypoint handoff, held-route completion, and idle-input cancellation defects are corrected. The stale window notice is non-blocking. Findings
SummaryAdds map-based routes with up to 20 waypoints, a companion pygame Route window, map overlays, and the Three navigation problems need correction before merging: intermediate handoffs emit stop commands, held routes can falsely complete an unvisited final leg, and idle zero input can still cancel and fail a route. Explicit viewer Stop can also be misreported as a waypoint failure. A separate, non-blocking window issue leaves a stale close notice over later failure explanations.
Reviews (1) · Last reviewed commit: "specs: multi-waypoint route feature" · Reviewed by Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
| self._route.mark_current_reached() | ||
| if status is RouteStatus.RUNNING: | ||
| self._issue_current_goal_locked() |
There was a problem hiding this comment.
Each intermediate waypoint publishes a fresh goal through the planner's cancel-and-restart interface. The actual planner emits two synchronous zero Twist commands at each handoff: one while cancelling the old path and another while starting its replacement. This sends stop commands between waypoints instead of preserving continuous pass-through motion.
This needs correction before merging. Use a motion-preserving path handoff for intermediate waypoints and cover it with a connected planner regression test. Physical stopping distance and speed were not measured.
Artifacts
Executed planner handoff harness
- The authored Python harness exercises real route and planner implementations with synthetic inputs and captures velocity messages and stacks, making the candidate reproducible without hardware.
Commands for the control and handoff runs
- The executed shell script runs both cases and records commands, working directories, exit codes, and source ranges, providing the exact reproduction procedure.
Planner output without intermediate goals
- The single-final-goal control received the same odometry as the route case and recorded no zero Twists in either observation window, establishing the comparison baseline.
Planner output during intermediate waypoint handoffs
- The three-waypoint route recorded replacement goals and two synchronous zero Twists at each intermediate handoff, confirming the candidate at the command boundary.
Relevant source ranges and unchanged production check
- Executed source-extraction commands captured waypoint advancement, planner cancellation, zero-Twist publication, blueprint wiring, and the pass-through requirement, linking the runtime evidence to unchanged production code.
Initial control attempt with grid-aligned odometry
- The initial control fixture entered final rotation rather than producing forward motion and exited 1, so it was not used as the final comparison.
Initial route attempt with grid-aligned odometry
- The initial route fixture also failed its forward-motion prerequisite and exited 1, documenting why the fixture was adjusted.
Threaded handoff attempt requiring motion recovery
- The intermediate handoff emitted the candidate's zero Twists but encountered a local-planner lifecycle error before recovering motion, motivating the final command-boundary assertions.
| if self._route.status in (RouteStatus.RUNNING, RouteStatus.HELD): | ||
| index = self._route.current_index | ||
| if index is not None and index == len(self._route.waypoints): | ||
| self._route.complete() |
There was a problem hiding this comment.
A held two-waypoint route can report completion without visiting waypoint 2. While the robot finishes waypoint 1, pass-radius processing advances current_index to 2 without publishing its goal. The planner's subsequent arrival result for waypoint 1 then satisfies this final-index check and completes the route. Start refuses to resume the unvisited leg because the route is already completed.
This needs correction before merging. Track the goal actually issued to the planner separately from the next waypoint, and apply arrival results only to that issued goal.
Artifacts
Targeted held-route reproduction source
- Executed this assertion-based script against real RouteFollower and Route code in two conditions, isolating the first-goal arrival callback as the trigger.
Commands and output-capture source
- Executed this runner to capture both reproduction commands and the production diff check with working directories and exit codes, making the evidence reproducible.
Held route resumes without the first-goal arrival callback
- Ran the control scenario without the arrival callback and then sent Start, showing waypoint 2 is published and remains resumable.
First-goal arrival falsely completes the held route
- Ran the same scenario with the first goal's True arrival callback and then sent Start, showing waypoint 2 falsely marked reached and resume refused.
Production files remain unchanged
- Executed git diff --exit-code for the RouteFollower and Route source files, confirming no production edits were made.
| if self._idle_twist_recent_locked(now): | ||
| return |
There was a problem hiding this comment.
Ignoring stop_seen after a recent zero twist does not prevent that input from cancelling navigation. MovementManager still cancels the planner, which emits goal_reached(False). When the follower has observed the zero twist before the stop notification, this filter leaves the route running until the cancellation result marks its waypoint failed. Idle zero traffic can therefore abort a route despite the intended filtering.
The viewer's explicit Stop publishes the same zero twist and can also be reported as a waypoint failure rather than an interruption. This needs correction before merging: filter idle input before planner cancellation, preserve explicit Stop intent, and correlate cancellation results so either delivery order is handled correctly.
Artifacts
- Executed actual navigation components over a controllable queued transport to expose the zero-stop cancellation behavior.
Executable validation commands
- Runs both connected comparisons and narrow existing tests while capturing commands, exit codes, and observed output.
Connected results with the zero-stop filter disabled
- Ran ten connected cases with only the early return removed in memory, showing interruption for stop-first delivery and failure for cancellation-result-first delivery.
Connected results with the current zero-stop filter
- Ran the same ten cases against unchanged current code, showing that both an observed zero Twist and explicit viewer Stop can mark the waypoint failed.
- Executed nine relevant route, movement-manager, and viewer Stop tests, all of which passed despite the connected defect.
Production source and revision capture
- Captured numbered source, hashes, revision, and tracked diff status to identify the tested implementation and verify no tracked production changes.
Comparison results and script hashes
- Extracted each case's observed route state, waypoint state, planner goal state, and cancellation result from the executed captures.
Initial harness scheduling error
- Preserves the initial failed run where the control handler's changed callback name prevented the intended delivery ordering; the harness was corrected before both successful comparison runs.
| if stop_enabled(self._state_doc): | ||
| self._notice = NOTICE_STOP_ROUTE_FIRST | ||
| return |
There was a problem hiding this comment.
A refused close leaves _notice set, and subsequent route-state updates never clear it. If the waypoint later fails, the message line keeps displaying “stop the route first” instead of the waypoint-failure explanation because message_text() always prefers the notice. The status and waypoint row still show failure, so this is a non-blocking feedback issue, but the stale instruction hides the reason.
Clear the close notice when the route leaves the active state or a new route message makes it obsolete.
Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!
Artifacts
▶ Waypoint failure without a close attempt
- Recorded the live pygame surface in Chromium while the route transitioned from running to failed without a close attempt—the waypoint-failure explanation remains visible.
▶ Waypoint failure after a refused close
- Recorded the live pygame surface through running, refused close, and failure—the obsolete stop instruction replaces the failure explanation.
Native pygame screenshot without a close attempt
- Saved the actual pygame display surface after the baseline failure—the message line renders the waypoint-failure explanation.
Native pygame screenshot after refused close and failure
- Saved the actual pygame display surface after the refused-close sequence—the failed state retains the stale stop instruction.
Executable pygame reproduction
- Executed this harness against the unchanged RouteControl implementation to deliver state updates, post the close event, assert rendered messages, and save native screenshots—the candidate is reproducible.
Executed Chromium capture script
- Executed this Playwright script to record both live pygame sequences and check their observed messages—the comparison uses real rendered pixels rather than a reconstructed UI.
Executed reproduction commands
- Ran this shell script under Xvfb to start the pygame harness and Chromium captures—both processes completed successfully.
Execution output for both rendered sequences
- Captured the command, working directory, exit code, incoming states, and actual drawn lines—the failure explanation is present in state but suppressed after refused close.
Unchanged-source and video verification output
- Ran git diff, git status, ffprobe, and artifact listing commands—the tracked source is unchanged and both recordings have valid durations.
|
Closing per request. |
Summary
Plan a route of up to 20 waypoints by clicking the map in
dimos-viewer, then drive it from a companion Route window. The robot visits the waypoints in order, passes through intermediate ones without stopping, and stops at the last. New modules and one new blueprint only; everything is additive (constitution VIII).RouteFollower(dimos/navigation/route_follower/) interposes on the viewer's map clicks, owns the route, feeds the existing planner one goal at a time ongoal_request, and detects completion, failure, operator stop and outside interruption (stop_movement/goal_requestfrom teleop, agents, patrol).RouteControlis a pygame Route window (add-waypoints switch, Start/Stop/Clear, per-row remove, status/target/message lines) speaking two JSON string streams, with a 1 Hz keepalive and a heartbeat that holds the route when the window goes silent.unitree-go2-routecomposes the Go2 smart stack with both modules, remaps the shared streams (contract:specs/001-multi-waypoint-route/contracts/route-streams.md) and merges the route overlay (numbered coloured waypoints + route line) into the Go2 rerun config.unitreeextra; docs at docs/capabilities/navigation/route.md.Safety (constitution III)
Edits never move the robot; only Start does. Stop reuses the shared
stop_movementstream and always works. Any outside goal or stop ends the route as interrupted. Window loss holds the robot after the current leg. Idle zero-twist teleop bursts are filtered before interrupting.Test plan
uv run pytest dimos/navigation/route_follower dimos/robot/unitree/go2/blueprints/smart: 106 passed (state machine, message documents, overlay, module behaviour incl. watchdog/interruption/failure, window behaviour headless, blueprint wiring incl. remappings and overlay merge).uv run pytest dimos/core dimos/navigation dimos/robot: 1208 passed (2 pre-existing environmental failures intest_git_cache.pyon this machine, unrelated).uv run mypy dimos/: only 2 pre-existing missing-stub errors in untouched files.Pending manual validation (fill in before merge)
specs/001-multi-waypoint-route/quickstart.md) in simulation and replay.specs/001-multi-waypoint-route/quickstart.md§4): TBDNotes
pytest dimos/robot/test_all_blueprints_generation.py.GIT_LFS_SKIP_PUSH=1); the branch adds no LFS-tracked files.