Reliability: never lose a cancel, no dead windows on fast switching, teleop/autonomy arbitration - #506
Merged
Merged
Conversation
…n grace, early physical feedback
Stress testing showed three ways fast Run/Stop switching breaks:
- A cancel landing in the first ~200ms of a code skill was silently lost:
skills reset self._cancelled = False as execute()'s first statement,
wiping a cancel that fired on another executor thread (6/20 lost in a
storm test; robot completes the full motion while reporting success).
All 21 fleet skills share the pattern. The base class now owns the flag:
_cancelled is a property over a threading.Event that latches True and
ignores False; the server re-arms it per run via _begin_run(), recovering
raced cancels from the goal handle's persistent cancel status. The latch
self-creates lazily because four skills skip super().__init__().
- Stop→Run hammering always rejected the new goal ("Another skill is
already running") while the old one tore down — a ~60ms window for code
skills, ~1.2s for physical ones. execute_callback now lets one goal wait
out a cancelling skill's teardown (2s cap, single waiter), including a
150ms beat for a cancel that arrives just after its follow-up Run.
- Physical skills published their first feedback only after the behavior
handshake (~1s), and rws can only bind an app cancel once that feedback
delivers the goal id — Stop was a silent no-op in that window. The
feedback now goes out before the handshake (+67ms measured).
Verified live on R7-27 over the app websocket: 30/30 immediate cancels
honored (was ~70%), Stop→Run accepted 13/13 (was 0/11).
…vigation, stop 4x faster on cancel - A mode switch tore down the Nav2 lifecycle under any active NavigateToPose goal; the goal never delivered a result, the navigating skill hung as 'running' forever, and the one-skill-at-a-time server rejected everything until a manual Stop. change_mode now cancels all active goals first (zeroed CancelGoal to the internal bt_navigator action = cancel-all), so they terminate through their normal result path — covers both skill navigation and map-page /goal_pose goals. - The same helper is exposed as /nav/cancel_navigation (std_srvs/Trigger) for the webapp map page's new Stop button: /goal_pose had no cancel affordance at all, and an unreachable goal ran spin/backup recoveries for ~40s with no way to stop it. - velocity_smoother max_decel -0.3/-0.5 kept the base moving 0.94-1.45s after a cancel result was already delivered. Decel is now -1.0/-2.0 (accel untouched — driving feel and goal approaches unchanged; the limit binds only when the command stream stops or drops). Measured tail after cancel: 0.25-0.35s. Validate on a powered base before fleet rollout — this robot's drivetrain was unpowered. Verified live: mode switch mid-navigation now yields a prompt terminal result; /nav/cancel_navigation cancels a /goal_pose run in 50ms.
…f fighting it Teleop (mars_app), skills (MobilityInterface, wheeled replay/learned playback) and Nav2 (velocity_smoother) all published /cmd_vel directly. A joystick nudge during navigation interleaved conflicting twists at the combined rate — measured 56 source flips in 2.2s, i.e. the base told 'rotate / forward / rotate / forward' ~25x/s. Each source now has its own topic and a small event-driven mux forwards the highest-priority fresh source to /cmd_vel: /cmd_vel_teleop > /cmd_vel_skills > /cmd_vel_nav -> /cmd_vel No re-timing or added latency; a single zero twist is published when all sources go stale so the base stops deterministically instead of waiting out its hardware deadman. The mux lives in mode_manager.launch.py so teleop keeps working with the nav stack down or mid-mode-switch. The recorder and base driver still see the final /cmd_vel unchanged; sim launch files are untouched (no mux in sim). Verified live: joystick during navigation now yields 0 source flips on /cmd_vel (teleop only), nav resumes when the stick is released, and teleop-only driving passes through unchanged.
The map page could start navigation (/goal_pose) but not stop it — an unreachable goal ran recovery motions for ~40s with no user recourse. Stop calls /nav/cancel_navigation, which cancels every active NavigateToPose goal server-side (including ones this page didn't start), then clears the local goal marker and route.
Contributor
Greptile SummaryThis PR improves robot cancellation, navigation stopping, and velocity arbitration. The main changes are:
Confidence Score: 5/5This looks safe to merge.
Important Files Changed
Reviews (6): Last reviewed commit: "refactor: flatten the reliability fixes ..." | Re-trigger Greptile |
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
karmanyaahm
approved these changes
Jul 8, 2026
Contributor
|
plz look at greptile stuff though |
karmanyaahm
previously requested changes
Jul 8, 2026
Contributor
There was a problem hiding this comment.
also integration test is failing
Already have image (with digest): gcr.io/cloud-builders/docker
--
=== /dev/shm ===
Filesystem Size Used Avail Use% Mounted on
shm 2.0G 0 2.0G 0% /dev/shm
=== exec-bit guard: install(PROGRAMS) node scripts ===
install(PROGRAMS) node scripts missing the exec bit (chmod +x them):
src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py
</div></div>
and see format test failing
- chmod +x cmd_vel_mux.py (install(PROGRAMS) exec-bit guard). - Rename unused loop var to _name (ruff B007); ruff format.
Addresses review (greptile P1s on mode_manager): - Cancel-ack != terminal: a non-empty goals_canceling only means the cancel request was accepted. Tearing Nav2 down then could deactivate bt_navigator before it delivered the cancelled goal's result, stranding the router/skill. Now subscribe to the bt_navigator action status and wait for the goals to reach a terminal state before returning. - Service-unavailable no longer reports success: 'is nav active' is decided from the action status topic (not the cancel service's momentary readiness), so a lifecycle-transition blip can't make us skip the cancel and silently strand a goal. If a goal is active but the cancel service is unreachable, return failure instead. Also drops an invalid '# noqa' directive in test_cancel_latch.py. Verified live: mode switch during nav delivers the nav result ~4s before change_mode returns (was: never); /nav/cancel_navigation reports 'Cancelled 1 navigation goal(s)' in 0.08s with motion stopping in 0.28s; at rest it returns 'No active navigation goals'.
Contributor
Author
|
@karmanyaahm addressed the review — pushed CI failures
Greptile P1s (both real, both fixed — replied inline with detail)
All three nav-cancel behaviors re-verified live on R7-27 (base unpowered, so goal-state machinery exercised at the action/topic level). |
…ability-fixes # Conflicts: # ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py
Contributor
Author
|
Merged latest main (resolved conflicts in Re-verified the full battery on the merged code, live on R7-27: lost-cancel storm 15/15 cancelled, Stop→Run grace accepted, mode-switch terminal-wait delivers the nav result ~4s before |
greptile follow-up P1: even with the terminal-wait, change_mode only logged and proceeded when _cancel_active_navigation returned False (cancel service unreachable, or goals not terminal in time), so Nav2 could still be torn down under a pending NavigateToPose goal — stranding the router/skill. Now it aborts the switch (returns failure, leaves nav running) in that case. The cancel request was still sent, so the goal terminates and delivers its result; the caller can Stop explicitly and retry once nav is idle. Verified live via a forced-failure build: with a nav goal active and cancel deliberately unconfirmable, change_mode returns 'Mode switch aborted: could not stop active navigation', the nav skill still gets its result (not stranded), and the mode stays 'navigation' (Nav2 not torn down). Happy path (cancel confirmed) still switches normally.
No behavior change — restructures the PR's additions for readability: - skills_server: the one-skill-at-a-time serialization is now a symmetric _claim_skill_slot / _release_skill_slot pair with the Stop→Run grace wait in its own _await_cancelling_teardown method; execute_callback reads linearly (parse → claim → run → release) instead of nesting the condition dance five levels deep. The duplicated initial-'running'-feedback block becomes _publish_initial_feedback. - mode_manager: _cancel_active_navigation reuses the file's existing call_service helper instead of a hand-rolled Event/future dance; the redundant lock around a single atomically-assigned int is gone; the cancel client lives in _service_clients like every other client; the duplicate local 'import threading' in __init__ is dropped. - cmd_vel_mux: the closure factory is replaced by a plain _on_twist method bound with functools.partial, with the priority check extracted to _fresh_source_above. Verified live after rebuild: cancel storm 12/12 cancelled, Stop→Run 4/4 accepted, mux 1 transition flip (baseline 56), mode-switch terminal-wait intact, /nav/cancel_navigation cancels an active goal in 0.12s; cancel-latch unit tests 6/6.
theo-michel
added a commit
that referenced
this pull request
Jul 8, 2026
…e reentrant map service Conflicts were both in mode_manager.py: imports (union of #506's QoS types and this branch's PointCloud2/sensor QoS) and the service-creation block (kept #506's cancel-all NavigateToPose machinery AND this branch's map service in the reentrant group + _mode_change_lock serialization). Semantics after merge: a mode change during a map change returns busy (#506's non-blocking acquire); a map change waits for a mode change — either way the two can no longer interleave lifecycle transitions. Live-verified post-merge: navigation mode boots, map switch + relocalization succeed, /nav/cancel_navigation is served, map-frame navigate_to_position reaches its goal.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Context
A live stress test on R7-27 (2026-07-07) drove the robot through the app's own rosbridge websocket — rapid Run/Stop toggling, cancel-mid-motion, mode switches during navigation, joystick during autonomy — and found five fixable defect classes in this repo (full report: internal artifact
stress-report, plusrwsbridge segfault tracked separately in innate-rws). This PR fixes all five, each verified live with the same harness that found it.Fixes & measured results
success); pattern shared by all 21 fleet skills_cancelledbecomes a platform-owned latch (threading.Event): latches True, ignores the skills'= Falsereset; server re-arms per run and recovers raced cancels from the goal handlechange_modecancels all active NavigateToPose goals first (cancel-all on the internal bt_navigator action)/nav/cancel_navigationTrigger service + Stop button on the map page/cmd_vel→ 56 source flips in 2.2 s (violent jerking on a powered base)cmd_vel_mux: event-driven priority mux,teleop > skills > nav; producers rerouted (mars_app, smoother remap,cmd_vel_topiclaunch arg, manipulation playback); zero-stop when all sources go stalemax_decel−0.3/−0.5)max_decel→ −1.0/−2.0 (accel untouched: driving feel and controller-planned goal approaches unchanged; binds only when the command stream stops/drops)Notes for review
self._cancelled = Falseresets harmless, and the latch self-creates lazily because four skills skipsuper().__init__(). Unit-tested intest/test_cancel_latch.py(6 tests).failure, notcancelled: the router can't legally mark its server goalcanceled()without a client cancel request (rclpy state machine), so it aborts. Truthful from the skill's perspective; left as-is./cmd_velstream level only.brain_client.sim.launch.pykeeps/cmd_vel, no mux in sim launch files.skills_serveridle CPU unchanged.Not in this PR
rws_serverSIGSEGV on client disconnect with an in-flight goal (100% reproducible; crashes the bridge for all clients) — lives ininnate-rws, repro + fix task filed separately.