From c84bb71b62f17266a6ca9d4592bf8f511136d86b Mon Sep 17 00:00:00 2001 From: Theo Michel Date: Tue, 7 Jul 2026 22:47:42 +0000 Subject: [PATCH 1/9] =?UTF-8?q?fix(brain):=20never=20lose=20a=20skill=20ca?= =?UTF-8?q?ncel=20=E2=80=94=20platform-owned=20latch,=20teardown=20grace,?= =?UTF-8?q?=20early=20physical=20feedback?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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). --- .../brain_client/nodes/skills_server.py | 77 ++++++++++--- .../brain_client/skills/invoker.py | 2 +- .../brain_client/brain_client/skills/types.py | 60 +++++++++- .../brain_client/test/test_cancel_latch.py | 106 ++++++++++++++++++ 4 files changed, 225 insertions(+), 20 deletions(-) create mode 100644 ros2_ws/src/brain/brain_client/test/test_cancel_latch.py diff --git a/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py b/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py index af12d3661..a2b97a593 100755 --- a/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py +++ b/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py @@ -79,6 +79,11 @@ def _coerce_numeric_inputs(skill, inputs: dict) -> dict: class SkillsActionServer(Node): + # How long a new goal may wait for a cancelling skill to finish tearing + # down before it is rejected (covers the ~1.2 s behavior-server cancel + # handshake of physical skills, with margin). + TEARDOWN_GRACE_SEC = 2.0 + def __init__(self): super().__init__("skills_action_server") @@ -133,8 +138,14 @@ def __init__(self): # The robot runs one skill at a time. Guards execute_callback so a goal # that arrives while another skill is still executing is aborted promptly # (see execute_callback) instead of contending for the arm/robot state. + # The condition variable lets one incoming goal wait briefly for a + # cancelling skill to finish tearing down (Stop→Run grace window) + # instead of being rejected during the handover. self._skill_execution_lock = threading.Lock() + self._skill_free = threading.Condition(self._skill_execution_lock) self._skill_running = False + self._active_goal_handle = None + self._teardown_waiter = False # ReentrantCallbackGroup so a cancel request can be serviced *while* a # skill's execute_callback is blocked waiting on the behavior result. @@ -285,12 +296,32 @@ def execute_callback(self, goal_handle): skill_type = goal_handle.request.skill_type # Serialize: the robot runs one skill at a time. If a goal arrives while - # another skill is still executing (e.g. rapid Run/Stop toggling, where - # the previous skill is mid-teardown), abort it here so the app gets a + # another skill is still executing, abort it here so the app gets a # prompt result. Rejecting in goal_callback instead would surface nothing # back through the rws bridge, leaving the app hung until its timeout. + # Exception: when the running skill is already being cancelled (rapid + # Run/Stop toggling — the user pressed Stop then immediately Run), one + # goal may wait out the teardown instead of failing; the measured window + # is ~60 ms for code skills and ~1.2 s for physical ones. Only one goal + # waits — a pile-up still gets the prompt abort. # No skill-status broadcast for the rejected goal — it never ran. - with self._skill_execution_lock: + with self._skill_free: + if self._skill_running and not self._teardown_waiter: + self._teardown_waiter = True + try: + # A Stop and its follow-up Run arrive back-to-back, and the + # new goal can beat the cancel into the server — give the + # cancel a beat to land, then wait out the teardown it + # triggers. A genuinely-busy robot (no cancel in flight) + # only delays its rejection by the first short wait. + self._skill_free.wait_for( + lambda: not self._skill_running or self._cancelling_teardown_in_progress(), + timeout=0.15, + ) + if self._skill_running and self._cancelling_teardown_in_progress(): + self._skill_free.wait_for(lambda: not self._skill_running, timeout=self.TEARDOWN_GRACE_SEC) + finally: + self._teardown_waiter = False if self._skill_running: self.get_logger().warn(f"Skill '{skill_type}' requested but another skill is already running") goal_handle.abort() @@ -301,6 +332,7 @@ def execute_callback(self, goal_handle): success_type=SkillResult.FAILURE.value, ) self._skill_running = True + self._active_goal_handle = goal_handle name = self._skill_display_name(skill_type) run_id = uuid.uuid4().hex @@ -326,8 +358,10 @@ def execute_callback(self, goal_handle): self._publish_skill_status(run_id, skill_type, name, "failed", str(e) or "internal error") raise finally: - with self._skill_execution_lock: + with self._skill_free: self._skill_running = False + self._active_goal_handle = None + self._skill_free.notify_all() status, reason = self._terminal_skill_status(result) self._publish_skill_status(run_id, skill_type, name, status, reason) return result @@ -399,7 +433,7 @@ def _publish_feedback(update_message: str, image_b64: str = None): goal_handle.publish_feedback(initial_feedback) self.robot_state.start_subscriptions() - result_message, result_status = self._run_code_skill_body(skill, skill_type, inputs) + result_message, result_status = self._run_code_skill_body(skill, skill_type, inputs, goal_handle) if result_status == SkillResult.SUCCESS: self.get_logger().info(f"Skill '{skill_type}' succeeded: {result_message}") @@ -431,7 +465,7 @@ def _publish_feedback(update_message: str, image_b64: str = None): finally: self.robot_state.stop_subscriptions() - def _run_code_skill_body(self, skill, skill_type, inputs): + def _run_code_skill_body(self, skill, skill_type, inputs, goal_handle): """Prepare robot state for a code skill and run its ``execute()``. Returns ``(message, SkillResult)``; goal finalization and subscriptions @@ -440,6 +474,14 @@ def _run_code_skill_body(self, skill, skill_type, inputs): state slot suspends/resumes (see RobotStateProvider) and the camera is refcounted. """ + # Arm the platform cancel latch for this run (recovers a cancel that + # raced goal startup — see Skill._begin_run), then honor it before any + # motion starts. + skill._begin_run(goal_handle) + if skill._cancelled: + self.get_logger().info(f"Skill '{skill_type}' cancelled before it started") + return "Skill cancelled before it started", SkillResult.CANCELLED + required_states = skill.get_required_robot_states() needs_camera = required_states and ( RobotStateType.LAST_MAIN_CAMERA_IMAGE_B64 in required_states @@ -501,6 +543,16 @@ def _run_physical_skill(self, goal_handle, skill_type, physical_data): """ metadata = physical_data["metadata"] try: + # Physical skills otherwise publish no feedback. Emit one immediately + # so the websocket bridge (rws) relays its assigned goal_id to the app + # — without it the app has no id to cancel/interrupt with. Sent before + # the behavior-server handshake (which takes up to ~1 s): a Stop + # pressed during the handshake must be able to bind to this goal. + initial_feedback = ExecuteSkill.Feedback() + initial_feedback.feedback = "running" + initial_feedback.image_b64 = "" + goal_handle.publish_feedback(initial_feedback) + if not self._behavior_client.wait_for_server(timeout_sec=5.0): self.get_logger().error("Behavior server not available!") return False, "Behavior server not available", SkillResult.FAILURE.value, "abort" @@ -531,14 +583,6 @@ def _run_physical_skill(self, goal_handle, skill_type, physical_data): self._register_behavior_goal_handle(goal_handle, behavior_goal_handle, skill_type) self.get_logger().info("Behavior goal accepted, waiting for result...") - # Physical skills otherwise publish no feedback. Emit one so the websocket - # bridge (rws) relays its assigned goal_id to the app — without it the app - # has no id to cancel/interrupt the running policy with. - initial_feedback = ExecuteSkill.Feedback() - initial_feedback.feedback = "running" - initial_feedback.image_b64 = "" - goal_handle.publish_feedback(initial_feedback) - result_future = behavior_goal_handle.get_result_async() result_wait_state = self._wait_for_cli_future(result_future, server_ready_check=self._behavior_server_ready) @@ -580,6 +624,11 @@ def _skill_goal_cancel_requested(self, goal_handle) -> bool: except Exception: return False + def _cancelling_teardown_in_progress(self) -> bool: + """True when the currently running skill has a cancel in flight, so an + incoming goal should wait out the teardown rather than be rejected.""" + return self._active_goal_handle is not None and self._skill_goal_cancel_requested(self._active_goal_handle) + def _register_behavior_goal_handle(self, skill_goal_handle, behavior_goal_handle, skill_type: str) -> None: key = self._skill_goal_key(skill_goal_handle) with self._behavior_goal_lock: diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/invoker.py b/ros2_ws/src/brain/brain_client/brain_client/skills/invoker.py index 446e05b04..65d28068b 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/invoker.py +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/invoker.py @@ -166,7 +166,7 @@ def _run_code(self, skill, skill_id, inputs): prev = self._active_code_skill self._active_code_skill = skill try: - return self._server._run_code_skill_body(skill, skill_id, inputs) + return self._server._run_code_skill_body(skill, skill_id, inputs, self._goal_handle) finally: self._active_code_skill = prev diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/types.py b/ros2_ws/src/brain/brain_client/brain_client/skills/types.py index ea77d2b39..86766cb69 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/types.py +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/types.py @@ -2,6 +2,7 @@ # Copyright (c) 2026 Innate Inc import json import os +import threading import time from abc import ABC, abstractmethod from enum import Enum @@ -196,6 +197,8 @@ def __init__(self, logger): self.logger = UniversalLogger(enabled=True, wrapped_logger=logger) self.node: Node | None = None self._feedback_callback = None + # Cancel latch, owned by the platform (see _cancelled/_begin_run below). + self._cancel_latch() # SkillInvoker for running other skills from execute(); injected by the # skills server before each run (see invoker.py and innate/skills.py). self.skills = None @@ -225,18 +228,65 @@ def execute(self, *args, **kwargs): """ pass + def _cancel_latch(self) -> threading.Event: + """The per-instance cancel event, created lazily: some skills define + __init__ without calling super().__init__(), so the base class can't + assume its own constructor ran. dict.setdefault keeps creation + race-free under the GIL.""" + latch = self.__dict__.get("_cancel_event") + if latch is None: + latch = self.__dict__.setdefault("_cancel_event", threading.Event()) + return latch + + @property + def _cancelled(self) -> bool: + """Whether cancellation was requested for the current run. + + Backed by a platform-owned latch: cancel() can fire on another executor + thread *before* execute() begins, and skills traditionally reset + ``self._cancelled = False`` as their first statement — wiping that early + cancel and running the full motion anyway. The setter below therefore + latches True but ignores False; only the skills server re-arms the latch + between runs (_begin_run), where a raced cancel is recovered from the + goal handle instead of lost. + """ + return self._cancel_latch().is_set() + + @_cancelled.setter + def _cancelled(self, value: bool): + if value: + self._cancel_latch().set() + + def _begin_run(self, goal_handle=None): + """Platform hook — the skills server calls this before each execute(). + + Re-arms the cancel latch for a fresh run, then re-latches immediately if + the goal was already cancel-requested: a cancel that raced goal startup + (or a clear() racing cancel()) is recovered from the goal's persistent + cancel status rather than dropped. + """ + latch = self._cancel_latch() + latch.clear() + try: + if goal_handle is not None and goal_handle.is_cancel_requested: + latch.set() + except Exception: + pass # duck-typed handles without cancel status + def cancel(self): """ Cancel the execution of the skill. Safe to call at any time; returns a message describing the result. - The default stops the child skill currently running via self.skills. - Override it to stop work of your own (motion, loops, timers) — and if - you also chain children, call self.skills.cancel() too. + The default latches the cancel flag (visible as self._cancelled) and + stops the child skill currently running via self.skills. Override it + to stop work of your own (motion, loops, timers) — and if you also + chain children, call self.skills.cancel() too. """ - if self.skills is not None: + self._cancel_latch().set() + if getattr(self, "skills", None) is not None: return self.skills.cancel() - return "Nothing to cancel" + return "Cancellation requested" @property def storage(self) -> SkillStorage: diff --git a/ros2_ws/src/brain/brain_client/test/test_cancel_latch.py b/ros2_ws/src/brain/brain_client/test/test_cancel_latch.py new file mode 100644 index 000000000..0870d5a1c --- /dev/null +++ b/ros2_ws/src/brain/brain_client/test/test_cancel_latch.py @@ -0,0 +1,106 @@ +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""Unit tests for the platform-owned skill cancel latch (Skill._cancelled). + +Pins the fix for the lost-cancel race: a cancel() that fires between goal +acceptance and execute() entry must survive the `self._cancelled = False` +reset that most skills perform as their first statement. +""" + +import logging +from types import SimpleNamespace + +from brain_client.skills.types import Skill, SkillResult + + +class LegacyPatternSkill(Skill): + """Mirrors the pattern used by the existing skill fleet: own _cancelled + flag, reset at execute() entry, set by an overridden cancel().""" + + def __init__(self): + super().__init__(logging.getLogger("test")) + self._cancelled = False + + @property + def name(self): + return "legacy_pattern" + + def execute(self): + self._cancelled = False # the reset that used to wipe an early cancel + if self._cancelled: + return "Cancelled", SkillResult.CANCELLED + return "Done", SkillResult.SUCCESS + + def cancel(self): + self._cancelled = True + return "cancelled" + + +def _goal_handle(cancel_requested): + return SimpleNamespace(is_cancel_requested=cancel_requested) + + +def test_cancel_before_execute_survives_reset(): + skill = LegacyPatternSkill() + skill._begin_run(_goal_handle(False)) + skill.cancel() # races ahead of execute() on another thread + skill.execute() # legacy reset must not clear the latch + assert skill._cancelled is True + + +def test_begin_run_clears_stale_latch_from_previous_run(): + skill = LegacyPatternSkill() + skill.cancel() # previous run was cancelled + skill._begin_run(_goal_handle(False)) + assert skill._cancelled is False + + +def test_begin_run_recovers_cancel_from_goal_handle(): + # cancel landed before _begin_run: the latch was set then cleared, but the + # goal's persistent cancel status re-latches it. + skill = LegacyPatternSkill() + skill.cancel() + skill._begin_run(_goal_handle(True)) + assert skill._cancelled is True + + +def test_setting_false_is_ignored_and_true_latches(): + skill = LegacyPatternSkill() + skill._begin_run(None) + skill._cancelled = False + assert skill._cancelled is False + skill._cancelled = True + skill._cancelled = False # mid-run reset attempts must not unlatch + assert skill._cancelled is True + + +def test_base_cancel_latches_without_invoker(): + skill = LegacyPatternSkill() + skill._begin_run(None) + Skill.cancel(skill) # base implementation, no self.skills set + assert skill._cancelled is True + + +class NoSuperInitSkill(Skill): + """Some fleet skills (navigate_to_position, send_email, …) define __init__ + without calling super().__init__() — the latch must self-create.""" + + def __init__(self): # noqa: intentionally no super().__init__() + self._cancelled = False + + @property + def name(self): + return "no_super_init" + + def execute(self): + return "Done", SkillResult.SUCCESS + + +def test_latch_works_without_super_init(): + skill = NoSuperInitSkill() + assert skill._cancelled is False + skill._begin_run(_goal_handle(False)) + Skill.cancel(skill) + assert skill._cancelled is True + skill._begin_run(_goal_handle(False)) + assert skill._cancelled is False From 083dc653919f7bca0754fae16e0f6356b07d8673 Mon Sep 17 00:00:00 2001 From: Theo Michel Date: Tue, 7 Jul 2026 22:47:53 +0000 Subject: [PATCH 2/9] fix(nav): cancel active navigation on mode switch, add /nav/cancel_navigation, stop 4x faster on cancel MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 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. --- .../mars_nav/config/velocity_smoother.yaml | 8 ++- .../mars_nav/mars_nav/mode_manager.py | 59 +++++++++++++++++++ 2 files changed, 66 insertions(+), 1 deletion(-) diff --git a/ros2_ws/src/mars_bot/mars_nav/config/velocity_smoother.yaml b/ros2_ws/src/mars_bot/mars_nav/config/velocity_smoother.yaml index 9d3e56bce..70e24acc0 100644 --- a/ros2_ws/src/mars_bot/mars_nav/config/velocity_smoother.yaml +++ b/ros2_ws/src/mars_bot/mars_nav/config/velocity_smoother.yaml @@ -11,7 +11,13 @@ velocity_smoother: min_velocity: [-0.2, 0.0, -0.6] # Based on controller min_vel_x, min_vel_y, symmetric min_vel_theta max_accel: [0.3, 0.0, 0.5] - max_decel: [-0.3, 0.0, -0.5] + # Decel deliberately harder than accel: it binds when the command stream + # stops or drops (cancel, preemption, velocity_timeout), where the old + # [-0.3, -0.5] kept the base moving 1.0-1.5 s after the app already showed + # "cancelled". Goal approaches still use the controller's gentler ax_min; + # this only makes Stop feel like stop (~0.3-0.45 s from full speed). + # Validate on a powered base before fleet rollout. + max_decel: [-1.0, 0.0, -2.0] deadband_velocity: [0.01, 0.0, 0.02] # Deadband for x, y, theta based on previous 'min_velocity_deadband' diff --git a/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py b/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py index 5876d0d47..7b768699b 100755 --- a/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py +++ b/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py @@ -6,6 +6,7 @@ import json import os import subprocess +import threading import time import traceback from enum import Enum @@ -14,6 +15,7 @@ # TF2 imports for transform lookup import tf2_ros +from action_msgs.srv import CancelGoal from brain_messages.srv import ChangeMap, ChangeNavigationMode, DeleteMap, SaveMap from geometry_msgs.msg import TransformStamped from lifecycle_msgs.msg import State @@ -25,6 +27,7 @@ from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from std_msgs.msg import String +from std_srvs.srv import Trigger from mars_nav.service_utils import call_service, get_node_state, transition_node @@ -108,6 +111,23 @@ def __init__(self): callback_group=self._internal_callbacks_group, ) + # Cancel every active NavigateToPose goal at the internal bt_navigator + # action (a zeroed CancelGoal request means "cancel all"). Both skill + # navigation (via navigate_to_pose_router) and map-page /goal_pose + # goals terminate through their normal result path, so callers waiting + # on a result unblock instead of hanging. + self._nav_cancel_client = self.create_client( + CancelGoal, + "/internal_navigate_to_pose/_action/cancel_goal", + callback_group=self._calls_going_outside_group, + ) + self.cancel_navigation_service = self.create_service( + Trigger, + "/nav/cancel_navigation", + self.cancel_navigation_callback, + callback_group=self._internal_callbacks_group, + ) + # Service to change maps in navigation mode self.map_service = self.create_service(ChangeMap, "/nav/change_navigation_map", self.change_map_callback) @@ -891,6 +911,36 @@ def _cleanup_orphaned_processes(self): except Exception as e: self.get_logger().warn(f"Cleanup warning: {e}") + def _cancel_active_navigation(self, timeout_sec=3.0): + """Cancel all active NavigateToPose goals. Returns (success, message). + + Sends a zeroed CancelGoal request ("cancel all") to the internal + bt_navigator action. Waits for the response so callers (mode switch, + the /nav/cancel_navigation service) know the goals are winding down + before tearing Nav2 down underneath them. + """ + if not self._nav_cancel_client.service_is_ready(): + return True, "Navigation is not running; nothing to cancel" + done = threading.Event() + future = self._nav_cancel_client.call_async(CancelGoal.Request()) + future.add_done_callback(lambda _f: done.set()) + if not done.wait(timeout_sec): + future.cancel() + return False, f"Timed out after {timeout_sec}s waiting for navigation cancel" + try: + cancelling = len(future.result().goals_canceling) + except Exception as e: + return False, f"Navigation cancel failed: {e}" + if cancelling: + return True, f"Cancelling {cancelling} active navigation goal(s)" + return True, "No active navigation goals" + + def cancel_navigation_callback(self, request, response): + """Trigger service: stop all active navigation (app Stop button).""" + response.success, response.message = self._cancel_active_navigation() + self.get_logger().info(f"/nav/cancel_navigation: {response.message}") + return response + def change_mode_callback(self, request, response, first_start=False): """ Service callback to switch between modes @@ -935,6 +985,15 @@ def change_mode_callback(self, request, response, first_start=False): self.get_logger().info(response.message) return response + # A mode switch tears down the Nav2 lifecycle under any active + # NavigateToPose goal, which would otherwise never deliver a result + # (the navigating skill hangs and the robot rejects all new skills + # until a manual Stop). Cancel active goals first so they terminate + # through their normal result path. + cancelled_ok, cancel_message = self._cancel_active_navigation() + if not cancelled_ok: + self.get_logger().warning(f"Proceeding with mode switch despite: {cancel_message}") + # Set mode to switching self.current_mode = "switching" self.publish_status() # Immediately publish the change From 4c4e5a75d4d3aad8465590e0c858b85228cfd972 Mon Sep 17 00:00:00 2001 From: Theo Michel Date: Tue, 7 Jul 2026 22:48:09 +0000 Subject: [PATCH 3/9] =?UTF-8?q?feat(nav):=20cmd=5Fvel=20priority=20mux=20?= =?UTF-8?q?=E2=80=94=20teleop=20overrides=20autonomy=20instead=20of=20figh?= =?UTF-8?q?ting=20it?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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. --- .../launch/brain_client.launch.py | 5 +- .../manipulation/manipulation_server.py | 4 +- .../mars_control/mars_control/app.cpp | 6 +- ros2_ws/src/mars_bot/mars_nav/CMakeLists.txt | 1 + .../mars_nav/launch/mode_manager.launch.py | 15 ++- .../mars_bot/mars_nav/mars_nav/cmd_vel_mux.py | 98 +++++++++++++++++++ 6 files changed, 124 insertions(+), 5 deletions(-) create mode 100644 ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py diff --git a/ros2_ws/src/brain/brain_client/launch/brain_client.launch.py b/ros2_ws/src/brain/brain_client/launch/brain_client.launch.py index bc37003c4..f75e98fc0 100644 --- a/ros2_ws/src/brain/brain_client/launch/brain_client.launch.py +++ b/ros2_ws/src/brain/brain_client/launch/brain_client.launch.py @@ -33,7 +33,9 @@ def generate_launch_description(): description="Image topic", ) cmd_vel_topic_arg = DeclareLaunchArgument( - "cmd_vel_topic", default_value="/cmd_vel", description="Command velocity topic" + "cmd_vel_topic", + default_value="/cmd_vel_skills", + description="Command velocity topic — skills-priority input of the cmd_vel mux", ) depth_image_topic_arg = DeclareLaunchArgument( "depth_image_topic", @@ -203,6 +205,7 @@ def generate_launch_description(): { "image_topic": LaunchConfiguration("image_topic"), "map_topic": LaunchConfiguration("map_topic"), + "cmd_vel_topic": LaunchConfiguration("cmd_vel_topic"), "simulator_mode": LaunchConfiguration("simulator_mode"), } ], diff --git a/ros2_ws/src/brain/manipulation/manipulation/manipulation_server.py b/ros2_ws/src/brain/manipulation/manipulation/manipulation_server.py index 82c1be7a7..a3f4c68f0 100755 --- a/ros2_ws/src/brain/manipulation/manipulation/manipulation_server.py +++ b/ros2_ws/src/brain/manipulation/manipulation/manipulation_server.py @@ -146,7 +146,9 @@ def __init__(self): self._joint_sub = None # Publishers - self.cmd_vel_pub = self.create_publisher(Twist, "/cmd_vel", 10) + # Base motion from wheeled replay/learned skills rides the skills input + # of the cmd_vel priority mux, so teleop can always override it. + self.cmd_vel_pub = self.create_publisher(Twist, "/cmd_vel_skills", 10) self.arm_state_pub = self.create_publisher(Float64MultiArray, "/mars/arm/commands", 10) # Head position command (degrees) — only published for head-enabled replay skills. self.head_set_position_pub = self.create_publisher(Int32, "/mars/head/set_position", 10) diff --git a/ros2_ws/src/mars_bot/mars_control/mars_control/app.cpp b/ros2_ws/src/mars_bot/mars_control/mars_control/app.cpp index f06986899..5c50f1e74 100644 --- a/ros2_ws/src/mars_bot/mars_control/mars_control/app.cpp +++ b/ros2_ws/src/mars_bot/mars_control/mars_control/app.cpp @@ -411,8 +411,10 @@ class AppControl : public rclcpp::Node { leader_sub_ = this->create_subscription( "/leader_positions", 10, std::bind(&AppControl::leader_positions_callback, this, std::placeholders::_1)); - // Publisher for velocity commands (Twist) on /cmd_vel - cmd_vel_pub_ = this->create_publisher("/cmd_vel", 10); + // Publisher for velocity commands (Twist). Teleop goes through the + // cmd_vel priority mux (mars_nav cmd_vel_mux.py), where it overrides + // autonomous sources instead of interleaving with them on /cmd_vel. + cmd_vel_pub_ = this->create_publisher("/cmd_vel_teleop", 10); // Publisher for leader arm commands (Float64MultiArray) on /mars/arm/commands cmd_pub_ = this->create_publisher("/mars/arm/commands", 10); diff --git a/ros2_ws/src/mars_bot/mars_nav/CMakeLists.txt b/ros2_ws/src/mars_bot/mars_nav/CMakeLists.txt index 65b46ce3b..6c8ca39fd 100644 --- a/ros2_ws/src/mars_bot/mars_nav/CMakeLists.txt +++ b/ros2_ws/src/mars_bot/mars_nav/CMakeLists.txt @@ -135,6 +135,7 @@ install(PROGRAMS mars_nav/mode_manager.py mars_nav/grid_localizer.py mars_nav/odom_to_tf_node.py + mars_nav/cmd_vel_mux.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py b/ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py index a1464e51c..f8c814953 100644 --- a/ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py +++ b/ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py @@ -102,10 +102,22 @@ def generate_launch_description(): name="velocity_smoother", output="screen", parameters=[smoother_params_file, smoother_limit_overrides], - remappings=[("cmd_vel", "/cmd_vel_scaled"), ("cmd_vel_smoothed", "/cmd_vel")], + # Nav2's smoothed output feeds the priority mux, not the base directly, + # so teleop can override autonomy without the two interleaving. + remappings=[("cmd_vel", "/cmd_vel_scaled"), ("cmd_vel_smoothed", "/cmd_vel_nav")], arguments=["--ros-args", "--log-level", "warn"], ) + # Single writer to /cmd_vel: teleop > skills > nav (see cmd_vel_mux.py). + # Lives here (not navigation.launch.py) because teleop must keep working + # when the nav stack is down or mid-mode-switch. + cmd_vel_mux_node = Node( + package="mars_nav", + executable="cmd_vel_mux.py", + name="cmd_vel_mux", + output="screen", + ) + # Shared BT navigator node bt_navigator_node = Node( package="nav2_bt_navigator", @@ -175,6 +187,7 @@ def generate_launch_description(): dynamic_footprint_node, goal_approach_scaler_node, velocity_smoother_node, + cmd_vel_mux_node, bt_navigator_node, mapfree_planner_node, behavior_server_node, diff --git a/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py b/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py new file mode 100644 index 000000000..a87534136 --- /dev/null +++ b/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 +# SPDX-License-Identifier: Apache-2.0 +# Copyright (c) 2026 Innate Inc +"""cmd_vel priority mux — one writer to the base at a time. + +Before this node, teleop (mars_app), skills (brain_client's MobilityInterface) +and Nav2 (velocity_smoother) all published /cmd_vel directly; a joystick nudge +during navigation interleaved conflicting twists at the combined publish rate +(measured: 56 source flips in 2.2 s — violent jerking on a powered base). + +Each source now publishes its own topic and the mux forwards messages from the +highest-priority source that has published within its freshness window: + + /cmd_vel_teleop (human override — wins over everything) + /cmd_vel_skills (code skills driving the base directly) + /cmd_vel_nav (Nav2 stack via the velocity smoother) + -> /cmd_vel (base driver) + +Forwarding is event-driven (no re-timing, no added latency). When every source +goes stale after motion, one zero twist is published so the base stops +deterministically instead of waiting out its hardware deadman. +""" + +import time + +import rclpy +from geometry_msgs.msg import Twist +from rclpy.node import Node + +# (name, topic, freshness window in seconds) — highest priority first. Windows +# comfortably exceed each source's publish interval (teleop ~10 Hz, skills' +# deadman refresh 20 Hz, smoother 40 Hz) so an active source never flickers +# stale between messages. +SOURCES = [ + ("teleop", "/cmd_vel_teleop", 0.5), + ("skills", "/cmd_vel_skills", 0.5), + ("nav", "/cmd_vel_nav", 0.5), +] + + +class CmdVelMux(Node): + def __init__(self): + super().__init__("cmd_vel_mux") + self._pub = self.create_publisher(Twist, "/cmd_vel", 10) + self._last_rx = {name: 0.0 for name, _topic, _window in SOURCES} + self._forwarding = False # motion since the last all-stale zero-stop + for index, (name, topic, _window) in enumerate(SOURCES): + self.create_subscription(Twist, topic, self._make_callback(index), 10) + self.create_timer(0.1, self._stop_when_all_stale) + self.get_logger().info( + "cmd_vel mux up: " + " > ".join(f"{name} ({topic})" for name, topic, _w in SOURCES) + ) + + def _make_callback(self, index): + name = SOURCES[index][0] + + def _on_twist(msg): + now = time.monotonic() + self._last_rx[name] = now + for higher_name, _topic, window in SOURCES[:index]: + if now - self._last_rx[higher_name] < window: + self.get_logger().info( + f"'{higher_name}' overriding '{name}' on /cmd_vel", + throttle_duration_sec=2.0, + ) + return + self._pub.publish(msg) + self._forwarding = True + + return _on_twist + + def _stop_when_all_stale(self): + """One deterministic zero after the last active source goes quiet.""" + if not self._forwarding: + return + now = time.monotonic() + if any(now - self._last_rx[name] < window for name, _topic, window in SOURCES): + return + self._forwarding = False + self._pub.publish(Twist()) + self.get_logger().debug("all cmd_vel sources stale — published zero stop") + + +def main(args=None): + rclpy.init(args=args) + node = CmdVelMux() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() From 3fde8d2c1e1cc1d10846455f2d4ee12055a84d83 Mon Sep 17 00:00:00 2001 From: Theo Michel Date: Tue, 7 Jul 2026 22:48:09 +0000 Subject: [PATCH 4/9] feat(webapp): Stop button on the map page MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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. --- webapp/css/app.css | 3 ++- webapp/js/constants.js | 4 ++++ webapp/js/map/mapWidget.js | 30 +++++++++++++++++++++++++++++- 3 files changed, 35 insertions(+), 2 deletions(-) diff --git a/webapp/css/app.css b/webapp/css/app.css index ca705c7fd..b38e5db73 100644 --- a/webapp/css/app.css +++ b/webapp/css/app.css @@ -272,7 +272,8 @@ button { gap: 8px; } -.map-goal-btn { +.map-goal-btn, +.map-stop-btn { background: rgba(10, 10, 12, 0.7); } diff --git a/webapp/js/constants.js b/webapp/js/constants.js index 6ce4321c1..86e380e87 100644 --- a/webapp/js/constants.js +++ b/webapp/js/constants.js @@ -61,6 +61,10 @@ export const PLAN_TOPIC = "/plan"; // nav_msgs/Path — the planner's route to t // planning; the resulting route streams back on PLAN_TOPIC. Same topic the sim // console's map view publishes to. export const GOAL_POSE_TOPIC = "/goal_pose"; +// Stop all active navigation (std_srvs/Trigger, served by mode_manager). +// Cancels every NavigateToPose goal — click-to-navigate ones and skill-driven +// ones alike — so the map page's Stop works no matter who started the motion. +export const CANCEL_NAVIGATION_SERVICE = "/nav/cancel_navigation"; // Skill-execution status (std_msgs/String JSON: {primitive_name|skill_name, // status: running|completed|failed|interrupted, primitive_id, ...}), published diff --git a/webapp/js/map/mapWidget.js b/webapp/js/map/mapWidget.js index 216914b61..c947e6d66 100644 --- a/webapp/js/map/mapWidget.js +++ b/webapp/js/map/mapWidget.js @@ -7,7 +7,7 @@ // is all a 2D map needs. import { ros } from "../rosClient.js"; -import { MAP_TOPIC, ODOM_TOPIC, PLAN_TOPIC, GOAL_POSE_TOPIC } from "../constants.js"; +import { MAP_TOPIC, ODOM_TOPIC, PLAN_TOPIC, GOAL_POSE_TOPIC, CANCEL_NAVIGATION_SERVICE } from "../constants.js"; // Wheel-zoom bounds (metres of real-world width shown). const MIN_ZOOM_M = 1; @@ -31,9 +31,13 @@ export function createMap(root, opts = {}) { const goalBtn = document.createElement("button"); goalBtn.className = "map-goal-btn sim-button"; goalBtn.textContent = "Set Goal"; + const stopBtn = document.createElement("button"); + stopBtn.className = "map-stop-btn sim-button"; + stopBtn.textContent = "Stop"; const controls = document.createElement("div"); controls.className = "map-controls"; controls.appendChild(goalBtn); + controls.appendChild(stopBtn); root.appendChild(controls); // Offscreen 1px-per-cell buffer; scaled to the canvas on draw (crisp + cheap). @@ -336,6 +340,30 @@ export function createMap(root, opts = {}) { } goalBtn.addEventListener("click", () => setGoalMode(!goalMode)); + + // Stop cancels every active navigation goal server-side — including ones this + // page didn't start (a skill's, another client's) — then drops the local goal + // marker and route. Button text doubles as the outcome indicator. + stopBtn.addEventListener("click", async () => { + stopBtn.disabled = true; + stopBtn.textContent = "Stopping…"; + try { + await ros.callService(CANCEL_NAVIGATION_SERVICE, {}); + goalMarker = null; + plan = null; + setGoalMode(false); + draw(); + stopBtn.textContent = "Stopped"; + } catch (err) { + console.error("[map] cancel navigation failed:", err); + stopBtn.textContent = "Stop failed"; + } finally { + stopBtn.disabled = false; + setTimeout(() => { + stopBtn.textContent = "Stop"; + }, 1500); + } + }); canvas.addEventListener("pointerdown", onPointerDown); canvas.addEventListener("pointermove", onPointerMove); canvas.addEventListener("pointerup", onPointerUp); From 14f1952162e5017082bdfe7e02730daed48b0f80 Mon Sep 17 00:00:00 2001 From: Theo Michel Date: Tue, 7 Jul 2026 23:50:02 +0000 Subject: [PATCH 5/9] chore: trim comments to essentials --- .../brain_client/nodes/skills_server.py | 34 +++++-------------- .../brain_client/brain_client/skills/types.py | 32 +++++------------ .../manipulation/manipulation_server.py | 3 +- .../mars_control/mars_control/app.cpp | 4 +-- .../mars_nav/config/velocity_smoother.yaml | 8 ++--- .../mars_nav/launch/mode_manager.launch.py | 8 ++--- .../mars_bot/mars_nav/mars_nav/cmd_vel_mux.py | 29 +++++----------- .../mars_nav/mars_nav/mode_manager.py | 23 ++++--------- webapp/js/constants.js | 5 ++- webapp/js/map/mapWidget.js | 5 ++- 10 files changed, 43 insertions(+), 108 deletions(-) diff --git a/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py b/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py index a2b97a593..461ca5a18 100755 --- a/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py +++ b/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py @@ -79,9 +79,7 @@ def _coerce_numeric_inputs(skill, inputs: dict) -> dict: class SkillsActionServer(Node): - # How long a new goal may wait for a cancelling skill to finish tearing - # down before it is rejected (covers the ~1.2 s behavior-server cancel - # handshake of physical skills, with margin). + # Max wait for a cancelling skill's teardown before a new goal is rejected. TEARDOWN_GRACE_SEC = 2.0 def __init__(self): @@ -138,9 +136,6 @@ def __init__(self): # The robot runs one skill at a time. Guards execute_callback so a goal # that arrives while another skill is still executing is aborted promptly # (see execute_callback) instead of contending for the arm/robot state. - # The condition variable lets one incoming goal wait briefly for a - # cancelling skill to finish tearing down (Stop→Run grace window) - # instead of being rejected during the handover. self._skill_execution_lock = threading.Lock() self._skill_free = threading.Condition(self._skill_execution_lock) self._skill_running = False @@ -299,21 +294,15 @@ def execute_callback(self, goal_handle): # another skill is still executing, abort it here so the app gets a # prompt result. Rejecting in goal_callback instead would surface nothing # back through the rws bridge, leaving the app hung until its timeout. - # Exception: when the running skill is already being cancelled (rapid - # Run/Stop toggling — the user pressed Stop then immediately Run), one - # goal may wait out the teardown instead of failing; the measured window - # is ~60 ms for code skills and ~1.2 s for physical ones. Only one goal - # waits — a pile-up still gets the prompt abort. + # Exception: one goal may wait out a cancelling skill's teardown (rapid + # Stop→Run), instead of failing with "already running". # No skill-status broadcast for the rejected goal — it never ran. with self._skill_free: if self._skill_running and not self._teardown_waiter: self._teardown_waiter = True try: - # A Stop and its follow-up Run arrive back-to-back, and the - # new goal can beat the cancel into the server — give the - # cancel a beat to land, then wait out the teardown it - # triggers. A genuinely-busy robot (no cancel in flight) - # only delays its rejection by the first short wait. + # The Run can beat its preceding Stop into the server; give + # the cancel a beat to land before deciding. self._skill_free.wait_for( lambda: not self._skill_running or self._cancelling_teardown_in_progress(), timeout=0.15, @@ -474,9 +463,6 @@ def _run_code_skill_body(self, skill, skill_type, inputs, goal_handle): state slot suspends/resumes (see RobotStateProvider) and the camera is refcounted. """ - # Arm the platform cancel latch for this run (recovers a cancel that - # raced goal startup — see Skill._begin_run), then honor it before any - # motion starts. skill._begin_run(goal_handle) if skill._cancelled: self.get_logger().info(f"Skill '{skill_type}' cancelled before it started") @@ -543,11 +529,8 @@ def _run_physical_skill(self, goal_handle, skill_type, physical_data): """ metadata = physical_data["metadata"] try: - # Physical skills otherwise publish no feedback. Emit one immediately - # so the websocket bridge (rws) relays its assigned goal_id to the app - # — without it the app has no id to cancel/interrupt with. Sent before - # the behavior-server handshake (which takes up to ~1 s): a Stop - # pressed during the handshake must be able to bind to this goal. + # Emit feedback before the behavior handshake: rws only relays the + # goal_id (which an app cancel must bind to) on the first feedback. initial_feedback = ExecuteSkill.Feedback() initial_feedback.feedback = "running" initial_feedback.image_b64 = "" @@ -625,8 +608,7 @@ def _skill_goal_cancel_requested(self, goal_handle) -> bool: return False def _cancelling_teardown_in_progress(self) -> bool: - """True when the currently running skill has a cancel in flight, so an - incoming goal should wait out the teardown rather than be rejected.""" + """True when the running skill has a cancel in flight.""" return self._active_goal_handle is not None and self._skill_goal_cancel_requested(self._active_goal_handle) def _register_behavior_goal_handle(self, skill_goal_handle, behavior_goal_handle, skill_type: str) -> None: diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/types.py b/ros2_ws/src/brain/brain_client/brain_client/skills/types.py index 86766cb69..55f5af5c7 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/types.py +++ b/ros2_ws/src/brain/brain_client/brain_client/skills/types.py @@ -197,7 +197,6 @@ def __init__(self, logger): self.logger = UniversalLogger(enabled=True, wrapped_logger=logger) self.node: Node | None = None self._feedback_callback = None - # Cancel latch, owned by the platform (see _cancelled/_begin_run below). self._cancel_latch() # SkillInvoker for running other skills from execute(); injected by the # skills server before each run (see invoker.py and innate/skills.py). @@ -229,10 +228,7 @@ def execute(self, *args, **kwargs): pass def _cancel_latch(self) -> threading.Event: - """The per-instance cancel event, created lazily: some skills define - __init__ without calling super().__init__(), so the base class can't - assume its own constructor ran. dict.setdefault keeps creation - race-free under the GIL.""" + """The cancel event, created lazily — some skills skip super().__init__().""" latch = self.__dict__.get("_cancel_event") if latch is None: latch = self.__dict__.setdefault("_cancel_event", threading.Event()) @@ -242,13 +238,9 @@ def _cancel_latch(self) -> threading.Event: def _cancelled(self) -> bool: """Whether cancellation was requested for the current run. - Backed by a platform-owned latch: cancel() can fire on another executor - thread *before* execute() begins, and skills traditionally reset - ``self._cancelled = False`` as their first statement — wiping that early - cancel and running the full motion anyway. The setter below therefore - latches True but ignores False; only the skills server re-arms the latch - between runs (_begin_run), where a raced cancel is recovered from the - goal handle instead of lost. + Latches True and ignores False: skills reset the flag at execute() + entry, which would wipe a cancel that raced goal startup. Only the + server re-arms it between runs (_begin_run). """ return self._cancel_latch().is_set() @@ -258,13 +250,8 @@ def _cancelled(self, value: bool): self._cancel_latch().set() def _begin_run(self, goal_handle=None): - """Platform hook — the skills server calls this before each execute(). - - Re-arms the cancel latch for a fresh run, then re-latches immediately if - the goal was already cancel-requested: a cancel that raced goal startup - (or a clear() racing cancel()) is recovered from the goal's persistent - cancel status rather than dropped. - """ + """Server hook: re-arm the latch for a fresh run, recovering a cancel + that already landed from the goal's persistent cancel status.""" latch = self._cancel_latch() latch.clear() try: @@ -278,10 +265,9 @@ def cancel(self): Cancel the execution of the skill. Safe to call at any time; returns a message describing the result. - The default latches the cancel flag (visible as self._cancelled) and - stops the child skill currently running via self.skills. Override it - to stop work of your own (motion, loops, timers) — and if you also - chain children, call self.skills.cancel() too. + The default latches self._cancelled and stops the child skill running + via self.skills. Override it to stop work of your own — and if you + also chain children, call self.skills.cancel() too. """ self._cancel_latch().set() if getattr(self, "skills", None) is not None: diff --git a/ros2_ws/src/brain/manipulation/manipulation/manipulation_server.py b/ros2_ws/src/brain/manipulation/manipulation/manipulation_server.py index a3f4c68f0..a0e14cbd9 100755 --- a/ros2_ws/src/brain/manipulation/manipulation/manipulation_server.py +++ b/ros2_ws/src/brain/manipulation/manipulation/manipulation_server.py @@ -146,8 +146,7 @@ def __init__(self): self._joint_sub = None # Publishers - # Base motion from wheeled replay/learned skills rides the skills input - # of the cmd_vel priority mux, so teleop can always override it. + # Skills input of the cmd_vel priority mux (teleop can override). self.cmd_vel_pub = self.create_publisher(Twist, "/cmd_vel_skills", 10) self.arm_state_pub = self.create_publisher(Float64MultiArray, "/mars/arm/commands", 10) # Head position command (degrees) — only published for head-enabled replay skills. diff --git a/ros2_ws/src/mars_bot/mars_control/mars_control/app.cpp b/ros2_ws/src/mars_bot/mars_control/mars_control/app.cpp index 5c50f1e74..12b9346b2 100644 --- a/ros2_ws/src/mars_bot/mars_control/mars_control/app.cpp +++ b/ros2_ws/src/mars_bot/mars_control/mars_control/app.cpp @@ -411,9 +411,7 @@ class AppControl : public rclcpp::Node { leader_sub_ = this->create_subscription( "/leader_positions", 10, std::bind(&AppControl::leader_positions_callback, this, std::placeholders::_1)); - // Publisher for velocity commands (Twist). Teleop goes through the - // cmd_vel priority mux (mars_nav cmd_vel_mux.py), where it overrides - // autonomous sources instead of interleaving with them on /cmd_vel. + // Teleop input of the cmd_vel priority mux (mars_nav cmd_vel_mux.py) cmd_vel_pub_ = this->create_publisher("/cmd_vel_teleop", 10); // Publisher for leader arm commands (Float64MultiArray) on /mars/arm/commands diff --git a/ros2_ws/src/mars_bot/mars_nav/config/velocity_smoother.yaml b/ros2_ws/src/mars_bot/mars_nav/config/velocity_smoother.yaml index 70e24acc0..8f599c69b 100644 --- a/ros2_ws/src/mars_bot/mars_nav/config/velocity_smoother.yaml +++ b/ros2_ws/src/mars_bot/mars_nav/config/velocity_smoother.yaml @@ -11,12 +11,8 @@ velocity_smoother: min_velocity: [-0.2, 0.0, -0.6] # Based on controller min_vel_x, min_vel_y, symmetric min_vel_theta max_accel: [0.3, 0.0, 0.5] - # Decel deliberately harder than accel: it binds when the command stream - # stops or drops (cancel, preemption, velocity_timeout), where the old - # [-0.3, -0.5] kept the base moving 1.0-1.5 s after the app already showed - # "cancelled". Goal approaches still use the controller's gentler ax_min; - # this only makes Stop feel like stop (~0.3-0.45 s from full speed). - # Validate on a powered base before fleet rollout. + # Decel harder than accel so cancels stop fast; goal approaches still use + # the controller's gentler ax_min. Validate on a powered base. max_decel: [-1.0, 0.0, -2.0] deadband_velocity: [0.01, 0.0, 0.02] # Deadband for x, y, theta based on previous 'min_velocity_deadband' diff --git a/ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py b/ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py index f8c814953..6f810ee53 100644 --- a/ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py +++ b/ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py @@ -102,15 +102,13 @@ def generate_launch_description(): name="velocity_smoother", output="screen", parameters=[smoother_params_file, smoother_limit_overrides], - # Nav2's smoothed output feeds the priority mux, not the base directly, - # so teleop can override autonomy without the two interleaving. + # Nav2 output feeds the priority mux, not the base directly. remappings=[("cmd_vel", "/cmd_vel_scaled"), ("cmd_vel_smoothed", "/cmd_vel_nav")], arguments=["--ros-args", "--log-level", "warn"], ) - # Single writer to /cmd_vel: teleop > skills > nav (see cmd_vel_mux.py). - # Lives here (not navigation.launch.py) because teleop must keep working - # when the nav stack is down or mid-mode-switch. + # Single writer to /cmd_vel: teleop > skills > nav. Lives here (always up) + # so teleop keeps working when the nav stack is down. cmd_vel_mux_node = Node( package="mars_nav", executable="cmd_vel_mux.py", diff --git a/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py b/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py index a87534136..1fc9ee9a5 100644 --- a/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py +++ b/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py @@ -3,22 +3,13 @@ # Copyright (c) 2026 Innate Inc """cmd_vel priority mux — one writer to the base at a time. -Before this node, teleop (mars_app), skills (brain_client's MobilityInterface) -and Nav2 (velocity_smoother) all published /cmd_vel directly; a joystick nudge -during navigation interleaved conflicting twists at the combined publish rate -(measured: 56 source flips in 2.2 s — violent jerking on a powered base). - -Each source now publishes its own topic and the mux forwards messages from the -highest-priority source that has published within its freshness window: - - /cmd_vel_teleop (human override — wins over everything) - /cmd_vel_skills (code skills driving the base directly) - /cmd_vel_nav (Nav2 stack via the velocity smoother) - -> /cmd_vel (base driver) - -Forwarding is event-driven (no re-timing, no added latency). When every source -goes stale after motion, one zero twist is published so the base stops -deterministically instead of waiting out its hardware deadman. +Forwards messages from the highest-priority source that has published within +its freshness window: + + /cmd_vel_teleop > /cmd_vel_skills > /cmd_vel_nav -> /cmd_vel + +Event-driven (no re-timing). One zero twist is published when every source +goes stale so the base stops deterministically. """ import time @@ -27,10 +18,8 @@ from geometry_msgs.msg import Twist from rclpy.node import Node -# (name, topic, freshness window in seconds) — highest priority first. Windows -# comfortably exceed each source's publish interval (teleop ~10 Hz, skills' -# deadman refresh 20 Hz, smoother 40 Hz) so an active source never flickers -# stale between messages. +# (name, topic, freshness window s) — highest priority first. Windows exceed +# each source's publish interval so an active source never flickers stale. SOURCES = [ ("teleop", "/cmd_vel_teleop", 0.5), ("skills", "/cmd_vel_skills", 0.5), diff --git a/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py b/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py index 7b768699b..702b69ffc 100755 --- a/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py +++ b/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py @@ -111,11 +111,8 @@ def __init__(self): callback_group=self._internal_callbacks_group, ) - # Cancel every active NavigateToPose goal at the internal bt_navigator - # action (a zeroed CancelGoal request means "cancel all"). Both skill - # navigation (via navigate_to_pose_router) and map-page /goal_pose - # goals terminate through their normal result path, so callers waiting - # on a result unblock instead of hanging. + # Cancel-all for NavigateToPose goals (skill nav and /goal_pose alike + # both terminate at the internal bt_navigator action). self._nav_cancel_client = self.create_client( CancelGoal, "/internal_navigate_to_pose/_action/cancel_goal", @@ -912,13 +909,8 @@ def _cleanup_orphaned_processes(self): self.get_logger().warn(f"Cleanup warning: {e}") def _cancel_active_navigation(self, timeout_sec=3.0): - """Cancel all active NavigateToPose goals. Returns (success, message). - - Sends a zeroed CancelGoal request ("cancel all") to the internal - bt_navigator action. Waits for the response so callers (mode switch, - the /nav/cancel_navigation service) know the goals are winding down - before tearing Nav2 down underneath them. - """ + """Cancel all active NavigateToPose goals (zeroed CancelGoal = cancel + all). Returns (success, message).""" if not self._nav_cancel_client.service_is_ready(): return True, "Navigation is not running; nothing to cancel" done = threading.Event() @@ -985,11 +977,8 @@ def change_mode_callback(self, request, response, first_start=False): self.get_logger().info(response.message) return response - # A mode switch tears down the Nav2 lifecycle under any active - # NavigateToPose goal, which would otherwise never deliver a result - # (the navigating skill hangs and the robot rejects all new skills - # until a manual Stop). Cancel active goals first so they terminate - # through their normal result path. + # Cancel active nav goals before tearing down their lifecycle nodes, + # or they never deliver a result and the navigating skill hangs. cancelled_ok, cancel_message = self._cancel_active_navigation() if not cancelled_ok: self.get_logger().warning(f"Proceeding with mode switch despite: {cancel_message}") diff --git a/webapp/js/constants.js b/webapp/js/constants.js index 86e380e87..bace5fc8b 100644 --- a/webapp/js/constants.js +++ b/webapp/js/constants.js @@ -61,9 +61,8 @@ export const PLAN_TOPIC = "/plan"; // nav_msgs/Path — the planner's route to t // planning; the resulting route streams back on PLAN_TOPIC. Same topic the sim // console's map view publishes to. export const GOAL_POSE_TOPIC = "/goal_pose"; -// Stop all active navigation (std_srvs/Trigger, served by mode_manager). -// Cancels every NavigateToPose goal — click-to-navigate ones and skill-driven -// ones alike — so the map page's Stop works no matter who started the motion. +// Stop all active navigation (std_srvs/Trigger) — cancels every NavigateToPose +// goal, no matter which client started it. export const CANCEL_NAVIGATION_SERVICE = "/nav/cancel_navigation"; // Skill-execution status (std_msgs/String JSON: {primitive_name|skill_name, diff --git a/webapp/js/map/mapWidget.js b/webapp/js/map/mapWidget.js index c947e6d66..f840e8229 100644 --- a/webapp/js/map/mapWidget.js +++ b/webapp/js/map/mapWidget.js @@ -341,9 +341,8 @@ export function createMap(root, opts = {}) { goalBtn.addEventListener("click", () => setGoalMode(!goalMode)); - // Stop cancels every active navigation goal server-side — including ones this - // page didn't start (a skill's, another client's) — then drops the local goal - // marker and route. Button text doubles as the outcome indicator. + // Stop cancels every active navigation goal server-side, then drops the + // local goal marker and route. stopBtn.addEventListener("click", async () => { stopBtn.disabled = true; stopBtn.textContent = "Stopping…"; From 9ca4c4351ca68329df766c306b66a9662d071fe5 Mon Sep 17 00:00:00 2001 From: Theo Michel Date: Wed, 8 Jul 2026 02:30:02 +0000 Subject: [PATCH 6/9] =?UTF-8?q?fix(nav):=20satisfy=20CI=20=E2=80=94=20exec?= =?UTF-8?q?=20bit=20+=20ruff=20on=20cmd=5Fvel=5Fmux.py?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - chmod +x cmd_vel_mux.py (install(PROGRAMS) exec-bit guard). - Rename unused loop var to _name (ruff B007); ruff format. --- ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) mode change 100644 => 100755 ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py diff --git a/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py b/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py old mode 100644 new mode 100755 index 1fc9ee9a5..44c37a0ef --- a/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py +++ b/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py @@ -33,12 +33,10 @@ def __init__(self): self._pub = self.create_publisher(Twist, "/cmd_vel", 10) self._last_rx = {name: 0.0 for name, _topic, _window in SOURCES} self._forwarding = False # motion since the last all-stale zero-stop - for index, (name, topic, _window) in enumerate(SOURCES): + for index, (_name, topic, _window) in enumerate(SOURCES): self.create_subscription(Twist, topic, self._make_callback(index), 10) self.create_timer(0.1, self._stop_when_all_stale) - self.get_logger().info( - "cmd_vel mux up: " + " > ".join(f"{name} ({topic})" for name, topic, _w in SOURCES) - ) + self.get_logger().info("cmd_vel mux up: " + " > ".join(f"{name} ({topic})" for name, topic, _w in SOURCES)) def _make_callback(self, index): name = SOURCES[index][0] From 01ca577a79bb00351cde328e781048bc88f8179b Mon Sep 17 00:00:00 2001 From: Theo Michel Date: Wed, 8 Jul 2026 02:30:02 +0000 Subject: [PATCH 7/9] fix(nav): wait for cancelled nav goals to terminate before mode teardown 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'. --- .../brain_client/test/test_cancel_latch.py | 3 +- .../mars_nav/mars_nav/mode_manager.py | 71 ++++++++++++++++--- 2 files changed, 64 insertions(+), 10 deletions(-) diff --git a/ros2_ws/src/brain/brain_client/test/test_cancel_latch.py b/ros2_ws/src/brain/brain_client/test/test_cancel_latch.py index 0870d5a1c..7bc044321 100644 --- a/ros2_ws/src/brain/brain_client/test/test_cancel_latch.py +++ b/ros2_ws/src/brain/brain_client/test/test_cancel_latch.py @@ -85,7 +85,8 @@ class NoSuperInitSkill(Skill): """Some fleet skills (navigate_to_position, send_email, …) define __init__ without calling super().__init__() — the latch must self-create.""" - def __init__(self): # noqa: intentionally no super().__init__() + def __init__(self): + # intentionally no super().__init__() — the latch must self-create self._cancelled = False @property diff --git a/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py b/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py index 702b69ffc..1306f14ce 100755 --- a/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py +++ b/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py @@ -15,6 +15,7 @@ # TF2 imports for transform lookup import tf2_ros +from action_msgs.msg import GoalStatus, GoalStatusArray from action_msgs.srv import CancelGoal from brain_messages.srv import ChangeMap, ChangeNavigationMode, DeleteMap, SaveMap from geometry_msgs.msg import TransformStamped @@ -26,6 +27,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy from std_msgs.msg import String from std_srvs.srv import Trigger @@ -118,6 +120,24 @@ def __init__(self): "/internal_navigate_to_pose/_action/cancel_goal", callback_group=self._calls_going_outside_group, ) + # Track how many bt_navigator goals are non-terminal, so cancellation + # can tell whether nav is active independent of service availability + # and can wait for goals to actually finish before Nav2 is torn down. + self._nav_active_goals = 0 + self._nav_status_lock = threading.Lock() + action_status_qos = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + self._nav_status_sub = self.create_subscription( + GoalStatusArray, + "/internal_navigate_to_pose/_action/status", + self._nav_status_callback, + action_status_qos, + callback_group=self._internal_callbacks_group, + ) self.cancel_navigation_service = self.create_service( Trigger, "/nav/cancel_navigation", @@ -908,24 +928,57 @@ def _cleanup_orphaned_processes(self): except Exception as e: self.get_logger().warn(f"Cleanup warning: {e}") - def _cancel_active_navigation(self, timeout_sec=3.0): - """Cancel all active NavigateToPose goals (zeroed CancelGoal = cancel - all). Returns (success, message).""" - if not self._nav_cancel_client.service_is_ready(): - return True, "Navigation is not running; nothing to cancel" + # bt_navigator goal states that are not yet terminal. + _NAV_ACTIVE_STATUSES = ( + GoalStatus.STATUS_ACCEPTED, + GoalStatus.STATUS_EXECUTING, + GoalStatus.STATUS_CANCELING, + ) + + def _nav_status_callback(self, msg): + active = sum(1 for s in msg.status_list if s.status in self._NAV_ACTIVE_STATUSES) + with self._nav_status_lock: + self._nav_active_goals = active + + def _active_nav_goal_count(self): + with self._nav_status_lock: + return self._nav_active_goals + + def _cancel_active_navigation(self, timeout_sec=5.0): + """Cancel all active NavigateToPose goals and wait for them to reach a + terminal state. Returns (success, message). + + Waits for terminal (not just cancel-acknowledged) so the caller can + tear Nav2 down without deactivating bt_navigator before it has + delivered the cancelled goal's result — which would strand the router + or skill waiting forever. + """ + if self._active_nav_goal_count() == 0: + return True, "No active navigation goals" + + # A goal is active, so the cancel service must be reachable — even if a + # lifecycle transition briefly hid it. Never report success on timeout: + # skipping the cancel would strand the goal once Nav2 is torn down. + if not self._nav_cancel_client.wait_for_service(timeout_sec=1.0): + return False, "Navigation active but cancel service is unavailable" + + deadline = time.time() + timeout_sec done = threading.Event() future = self._nav_cancel_client.call_async(CancelGoal.Request()) future.add_done_callback(lambda _f: done.set()) if not done.wait(timeout_sec): future.cancel() - return False, f"Timed out after {timeout_sec}s waiting for navigation cancel" + return False, "Timed out waiting for cancel acknowledgement" try: cancelling = len(future.result().goals_canceling) except Exception as e: return False, f"Navigation cancel failed: {e}" - if cancelling: - return True, f"Cancelling {cancelling} active navigation goal(s)" - return True, "No active navigation goals" + + while self._active_nav_goal_count() > 0 and time.time() < deadline: + time.sleep(0.05) + if self._active_nav_goal_count() > 0: + return False, "Cancelled goals did not reach a terminal state in time" + return True, f"Cancelled {cancelling} navigation goal(s)" def cancel_navigation_callback(self, request, response): """Trigger service: stop all active navigation (app Stop button).""" From 6723344e3dec60415f5348530512ff1ec7a6cd7d Mon Sep 17 00:00:00 2001 From: Theo Michel Date: Wed, 8 Jul 2026 03:07:32 +0000 Subject: [PATCH 8/9] fix(nav): abort mode switch when active navigation can't be stopped MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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. --- .../src/mars_bot/mars_nav/mars_nav/mode_manager.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py b/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py index 1306f14ce..5af7e6d26 100755 --- a/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py +++ b/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py @@ -1030,11 +1030,17 @@ def change_mode_callback(self, request, response, first_start=False): self.get_logger().info(response.message) return response - # Cancel active nav goals before tearing down their lifecycle nodes, - # or they never deliver a result and the navigating skill hangs. + # Cancel active nav goals before tearing down their lifecycle nodes. + # If cancellation can't be confirmed (service unreachable, or goals + # not terminal in time), abort rather than tear Nav2 down under a + # pending goal — that would strand the router/skill forever. Nav is + # left running; the caller can Stop explicitly and retry. cancelled_ok, cancel_message = self._cancel_active_navigation() if not cancelled_ok: - self.get_logger().warning(f"Proceeding with mode switch despite: {cancel_message}") + response.success = False + response.message = f"Mode switch aborted: could not stop active navigation ({cancel_message})" + self.get_logger().error(response.message) + return response # Set mode to switching self.current_mode = "switching" From bada307dcaef1ffb528cdc12cf32efcaadf73d71 Mon Sep 17 00:00:00 2001 From: Theo Michel Date: Wed, 8 Jul 2026 03:30:33 +0000 Subject: [PATCH 9/9] refactor: flatten the reliability fixes (review feedback) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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. --- .../brain_client/nodes/skills_server.py | 123 ++++++++++-------- .../mars_bot/mars_nav/mars_nav/cmd_vel_mux.py | 38 +++--- .../mars_nav/mars_nav/mode_manager.py | 68 ++++------ 3 files changed, 110 insertions(+), 119 deletions(-) diff --git a/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py b/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py index b3a6c8e56..1b9a8da62 100755 --- a/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py +++ b/ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py @@ -312,38 +312,16 @@ def execute_callback(self, goal_handle): skill_type = goal_handle.request.skill_type - # Serialize: the robot runs one skill at a time. If a goal arrives while - # another skill is still executing, abort it here so the app gets a - # prompt result. Rejecting in goal_callback instead would surface nothing - # back through the rws bridge, leaving the app hung until its timeout. - # Exception: one goal may wait out a cancelling skill's teardown (rapid - # Stop→Run), instead of failing with "already running". - # No skill-status broadcast for the rejected goal — it never ran. - with self._skill_free: - if self._skill_running and not self._teardown_waiter: - self._teardown_waiter = True - try: - # The Run can beat its preceding Stop into the server; give - # the cancel a beat to land before deciding. - self._skill_free.wait_for( - lambda: not self._skill_running or self._cancelling_teardown_in_progress(), - timeout=0.15, - ) - if self._skill_running and self._cancelling_teardown_in_progress(): - self._skill_free.wait_for(lambda: not self._skill_running, timeout=self.TEARDOWN_GRACE_SEC) - finally: - self._teardown_waiter = False - if self._skill_running: - self.get_logger().warn(f"Skill '{skill_type}' requested but another skill is already running") - goal_handle.abort() - return ExecuteSkill.Result( - success=False, - message="Another skill is already running", - skill_type=skill_type, - success_type=SkillResult.FAILURE.value, - ) - self._skill_running = True - self._active_goal_handle = goal_handle + # No skill-status broadcast for a refused goal — it never ran. + if not self._claim_skill_slot(goal_handle): + self.get_logger().warn(f"Skill '{skill_type}' requested but another skill is already running") + goal_handle.abort() + return ExecuteSkill.Result( + success=False, + message="Another skill is already running", + skill_type=skill_type, + success_type=SkillResult.FAILURE.value, + ) name = self._skill_display_name(skill_type) run_id = uuid.uuid4().hex @@ -369,16 +347,21 @@ def execute_callback(self, goal_handle): self._publish_skill_status(run_id, skill_type, name, "failed", str(e) or "internal error") raise finally: - with self._skill_free: - self._skill_running = False - self._active_goal_handle = None - retired, self._pending_retired_skills = self._pending_retired_skills, [] - self._skill_free.notify_all() - SkillRepository.dispose_instances(retired, self.get_logger()) + self._release_skill_slot() status, reason = self._terminal_skill_status(result) self._publish_skill_status(run_id, skill_type, name, status, reason) return result + @staticmethod + def _publish_initial_feedback(goal_handle): + """Emit one "running" feedback the moment execution starts: rws only + relays its assigned goal_id — which an app cancel must bind to — on the + first feedback, and skills may produce none of their own for a while.""" + feedback = ExecuteSkill.Feedback() + feedback.feedback = "running" + feedback.image_b64 = "" + goal_handle.publish_feedback(feedback) + def _skill_display_name(self, skill_type: str) -> str: code_entry = self.catalog.get_code_skill(skill_type) if code_entry is not None: @@ -436,15 +419,7 @@ def _publish_feedback(update_message: str, image_b64: str = None): skill.skills = SkillInvoker(self, goal_handle, _publish_feedback) try: - # Code skills otherwise publish no feedback until they call their own - # feedback callback. Emit one so the websocket bridge (rws) relays its - # assigned goal_id to the app — without it the app has no id to - # cancel/interrupt the running skill with. - initial_feedback = ExecuteSkill.Feedback() - initial_feedback.feedback = "running" - initial_feedback.image_b64 = "" - goal_handle.publish_feedback(initial_feedback) - + self._publish_initial_feedback(goal_handle) self.robot_state.start_subscriptions() result_message, result_status = self._run_code_skill_body(skill, skill_type, inputs, goal_handle) @@ -553,13 +528,7 @@ def _run_physical_skill(self, goal_handle, skill_type, physical_data): """ metadata = physical_data["metadata"] try: - # Emit feedback before the behavior handshake: rws only relays the - # goal_id (which an app cancel must bind to) on the first feedback. - initial_feedback = ExecuteSkill.Feedback() - initial_feedback.feedback = "running" - initial_feedback.image_b64 = "" - goal_handle.publish_feedback(initial_feedback) - + self._publish_initial_feedback(goal_handle) if not self._behavior_client.wait_for_server(timeout_sec=5.0): self.get_logger().error("Behavior server not available!") return False, "Behavior server not available", SkillResult.FAILURE.value, "abort" @@ -635,6 +604,52 @@ def _cancelling_teardown_in_progress(self) -> bool: """True when the running skill has a cancel in flight.""" return self._active_goal_handle is not None and self._skill_goal_cancel_requested(self._active_goal_handle) + def _claim_skill_slot(self, goal_handle) -> bool: + """Claim the one-skill-at-a-time slot; False means another skill kept it. + + A goal that arrives while another skill is executing is refused — the + caller aborts it so the app gets a prompt result (rejecting in + goal_callback instead would surface nothing back through the rws + bridge). One exception: rapid Stop→Run, where the previous skill is + mid-teardown from a cancel — that goal waits the teardown out instead + of failing with "already running". + """ + with self._skill_free: + if self._skill_running: + self._await_cancelling_teardown() + if self._skill_running: + return False + self._skill_running = True + self._active_goal_handle = goal_handle + return True + + def _await_cancelling_teardown(self): + """Wait for a cancelling skill to release the slot. Call holding _skill_free. + + Only one goal waits; a pile-up keeps the prompt rejection. The first + short wait covers a Run that beat its preceding Stop into the server. + """ + if self._teardown_waiter: + return + self._teardown_waiter = True + try: + self._skill_free.wait_for( + lambda: not self._skill_running or self._cancelling_teardown_in_progress(), + timeout=0.15, + ) + if self._cancelling_teardown_in_progress(): + self._skill_free.wait_for(lambda: not self._skill_running, timeout=self.TEARDOWN_GRACE_SEC) + finally: + self._teardown_waiter = False + + def _release_skill_slot(self): + with self._skill_free: + self._skill_running = False + self._active_goal_handle = None + retired, self._pending_retired_skills = self._pending_retired_skills, [] + self._skill_free.notify_all() + SkillRepository.dispose_instances(retired, self.get_logger()) + def _register_behavior_goal_handle(self, skill_goal_handle, behavior_goal_handle, skill_type: str) -> None: key = self._skill_goal_key(skill_goal_handle) with self._behavior_goal_lock: diff --git a/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py b/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py index 44c37a0ef..5cc450829 100755 --- a/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py +++ b/ros2_ws/src/mars_bot/mars_nav/mars_nav/cmd_vel_mux.py @@ -13,6 +13,7 @@ """ import time +from functools import partial import rclpy from geometry_msgs.msg import Twist @@ -33,28 +34,27 @@ def __init__(self): self._pub = self.create_publisher(Twist, "/cmd_vel", 10) self._last_rx = {name: 0.0 for name, _topic, _window in SOURCES} self._forwarding = False # motion since the last all-stale zero-stop - for index, (_name, topic, _window) in enumerate(SOURCES): - self.create_subscription(Twist, topic, self._make_callback(index), 10) + for priority, (name, topic, _window) in enumerate(SOURCES): + self.create_subscription(Twist, topic, partial(self._on_twist, priority, name), 10) self.create_timer(0.1, self._stop_when_all_stale) self.get_logger().info("cmd_vel mux up: " + " > ".join(f"{name} ({topic})" for name, topic, _w in SOURCES)) - def _make_callback(self, index): - name = SOURCES[index][0] - - def _on_twist(msg): - now = time.monotonic() - self._last_rx[name] = now - for higher_name, _topic, window in SOURCES[:index]: - if now - self._last_rx[higher_name] < window: - self.get_logger().info( - f"'{higher_name}' overriding '{name}' on /cmd_vel", - throttle_duration_sec=2.0, - ) - return - self._pub.publish(msg) - self._forwarding = True - - return _on_twist + def _on_twist(self, priority, name, msg): + now = time.monotonic() + self._last_rx[name] = now + override = self._fresh_source_above(priority, now) + if override: + self.get_logger().info(f"'{override}' overriding '{name}' on /cmd_vel", throttle_duration_sec=2.0) + return + self._pub.publish(msg) + self._forwarding = True + + def _fresh_source_above(self, priority, now): + """Name of a higher-priority source still inside its freshness window.""" + for name, _topic, window in SOURCES[:priority]: + if now - self._last_rx[name] < window: + return name + return None def _stop_when_all_stale(self): """One deterministic zero after the last active source goes quiet.""" diff --git a/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py b/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py index 5af7e6d26..8ba909db8 100755 --- a/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py +++ b/ros2_ws/src/mars_bot/mars_nav/mars_nav/mode_manager.py @@ -27,7 +27,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy +from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy from std_msgs.msg import String from std_srvs.srv import Trigger @@ -37,6 +37,8 @@ map_server_node = "navigation_map_server" bt_node = "bt_navigator" +NAV_CANCEL_SERVICE = "/internal_navigate_to_pose/_action/cancel_goal" + # Nodes that should only be configured (not activated) in specific modes configure_only_nodes = { "mapfree": {"navigation/planner_server"}, @@ -98,7 +100,6 @@ def __init__(self): self._executor = None # Lock to prevent concurrent mode changes - import threading self._mode_change_lock = threading.Lock() @@ -114,28 +115,19 @@ def __init__(self): ) # Cancel-all for NavigateToPose goals (skill nav and /goal_pose alike - # both terminate at the internal bt_navigator action). - self._nav_cancel_client = self.create_client( - CancelGoal, - "/internal_navigate_to_pose/_action/cancel_goal", - callback_group=self._calls_going_outside_group, - ) - # Track how many bt_navigator goals are non-terminal, so cancellation - # can tell whether nav is active independent of service availability - # and can wait for goals to actually finish before Nav2 is torn down. + # both terminate at the internal bt_navigator action). Goal activity is + # tracked from the action's status topic, not service availability. self._nav_active_goals = 0 - self._nav_status_lock = threading.Lock() - action_status_qos = QoSProfile( - depth=1, - history=QoSHistoryPolicy.KEEP_LAST, - reliability=QoSReliabilityPolicy.RELIABLE, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + self._service_clients[NAV_CANCEL_SERVICE] = self.create_client( + CancelGoal, NAV_CANCEL_SERVICE, callback_group=self._calls_going_outside_group ) self._nav_status_sub = self.create_subscription( GoalStatusArray, "/internal_navigate_to_pose/_action/status", self._nav_status_callback, - action_status_qos, + QoSProfile( + depth=1, reliability=QoSReliabilityPolicy.RELIABLE, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL + ), callback_group=self._internal_callbacks_group, ) self.cancel_navigation_service = self.create_service( @@ -936,13 +928,7 @@ def _cleanup_orphaned_processes(self): ) def _nav_status_callback(self, msg): - active = sum(1 for s in msg.status_list if s.status in self._NAV_ACTIVE_STATUSES) - with self._nav_status_lock: - self._nav_active_goals = active - - def _active_nav_goal_count(self): - with self._nav_status_lock: - return self._nav_active_goals + self._nav_active_goals = sum(1 for s in msg.status_list if s.status in self._NAV_ACTIVE_STATUSES) def _cancel_active_navigation(self, timeout_sec=5.0): """Cancel all active NavigateToPose goals and wait for them to reach a @@ -951,34 +937,24 @@ def _cancel_active_navigation(self, timeout_sec=5.0): Waits for terminal (not just cancel-acknowledged) so the caller can tear Nav2 down without deactivating bt_navigator before it has delivered the cancelled goal's result — which would strand the router - or skill waiting forever. + or skill waiting forever. Never reports success while a goal is still + live: skipping the cancel would strand it once Nav2 is torn down. """ - if self._active_nav_goal_count() == 0: + if self._nav_active_goals == 0: return True, "No active navigation goals" - # A goal is active, so the cancel service must be reachable — even if a - # lifecycle transition briefly hid it. Never report success on timeout: - # skipping the cancel would strand the goal once Nav2 is torn down. - if not self._nav_cancel_client.wait_for_service(timeout_sec=1.0): - return False, "Navigation active but cancel service is unavailable" - deadline = time.time() + timeout_sec - done = threading.Event() - future = self._nav_cancel_client.call_async(CancelGoal.Request()) - future.add_done_callback(lambda _f: done.set()) - if not done.wait(timeout_sec): - future.cancel() - return False, "Timed out waiting for cancel acknowledgement" - try: - cancelling = len(future.result().goals_canceling) - except Exception as e: - return False, f"Navigation cancel failed: {e}" + response = call_service( + self._service_clients, self.get_logger(), NAV_CANCEL_SERVICE, CancelGoal.Request(), timeout_sec + ) + if response is None: + return False, "Navigation is active but its cancel service did not respond" - while self._active_nav_goal_count() > 0 and time.time() < deadline: + while self._nav_active_goals > 0 and time.time() < deadline: time.sleep(0.05) - if self._active_nav_goal_count() > 0: + if self._nav_active_goals > 0: return False, "Cancelled goals did not reach a terminal state in time" - return True, f"Cancelled {cancelling} navigation goal(s)" + return True, f"Cancelled {len(response.goals_canceling)} navigation goal(s)" def cancel_navigation_callback(self, request, response): """Trigger service: stop all active navigation (app Stop button)."""