Reliability: never lose a cancel, no dead windows on fast switching, teleop/autonomy arbitration

ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py

@@ -79,6 +79,9 @@ def _coerce_numeric_inputs(skill, inputs: dict) -> dict:
 
 
 class SkillsActionServer(Node):
+    # Max wait for a cancelling skill's teardown before a new goal is rejected.
+    TEARDOWN_GRACE_SEC = 2.0
+
     def __init__(self):
         super().__init__("skills_action_server")
 
@@ -112,11 +115,17 @@ 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.
         # Also gates disposal of reload-retired skill instances: destroying a
         # retired instance's ROS entities mid-run would crash the skill that is
         # still spinning them, so disposal waits until execution ends.
         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
         self._pending_retired_skills = []
 
         # Skill catalog (discovery, metadata, publishing, reload).
@@ -304,12 +313,26 @@ 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: 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_execution_lock:
+        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()
@@ -320,6 +343,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
@@ -345,9 +369,11 @@ 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
                 retired, self._pending_retired_skills = self._pending_retired_skills, []
+                self._skill_free.notify_all()
             SkillRepository.dispose_instances(retired, self.get_logger())
         status, reason = self._terminal_skill_status(result)
         self._publish_skill_status(run_id, skill_type, name, status, reason)
@@ -420,7 +446,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}")
@@ -452,7 +478,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
@@ -461,6 +487,11 @@ def _run_code_skill_body(self, skill, skill_type, inputs):
         state slot suspends/resumes (see RobotStateProvider) and the camera is
         refcounted.
         """
+        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
@@ -522,6 +553,13 @@ 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)
+
             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"
@@ -552,14 +590,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)
 
@@ -601,6 +631,10 @@ def _skill_goal_cancel_requested(self, goal_handle) -> bool:
         except Exception:
             return False
 
+    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 _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:

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
 

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,7 @@ def __init__(self, logger):
         self.logger = UniversalLogger(enabled=True, wrapped_logger=logger)
         self.node: Node | None = None
         self._feedback_callback = None
+        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 +227,52 @@ def execute(self, *args, **kwargs):
         """
         pass
 
+    def _cancel_latch(self) -> threading.Event:
+        """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())
+        return latch
+
+    @property
+    def _cancelled(self) -> bool:
+        """Whether cancellation was requested for the current run.
+
+        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()
+
+    @_cancelled.setter
+    def _cancelled(self, value: bool):
+        if value:
+            self._cancel_latch().set()
+
+    def _begin_run(self, goal_handle=None):
+        """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:
+            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 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.
         """
-        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"
 
     def shutdown(self):  # noqa: B027
         """Release resources this instance owns. Called when the server retires

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"),
                     }
                 ],

ros2_ws/src/brain/brain_client/test/test_cancel_latch.py

@@ -0,0 +1,107 @@
+# 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):
+        # intentionally no super().__init__() — the latch must self-create
+        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

ros2_ws/src/brain/manipulation/manipulation/manipulation_server.py

@@ -146,7 +146,8 @@ def __init__(self):
         self._joint_sub = None
 
         # Publishers
-        self.cmd_vel_pub = self.create_publisher(Twist, "/cmd_vel", 10)
+        # 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.
         self.head_set_position_pub = self.create_publisher(Int32, "/mars/head/set_position", 10)

ros2_ws/src/mars_bot/mars_control/mars_control/app.cpp

@@ -411,8 +411,8 @@ class AppControl : public rclcpp::Node {
         leader_sub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>(
             "/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<geometry_msgs::msg::Twist>("/cmd_vel", 10);
+        // Teleop input of the cmd_vel priority mux (mars_nav cmd_vel_mux.py)
+        cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel_teleop", 10);
 
         // Publisher for leader arm commands (Float64MultiArray) on /mars/arm/commands
         cmd_pub_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("/mars/arm/commands", 10);

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}
 )
 

ros2_ws/src/mars_bot/mars_nav/config/velocity_smoother.yaml

@@ -11,7 +11,9 @@ 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 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'