fix(brain): stop destroying ROS entities under spinning executors (skills-server cancel crash-loop)

ros2_ws/src/brain/brain_client/brain_client/core/lifecycle.py

@@ -160,6 +160,8 @@ def reactivate_brain(self) -> None:
         # Re-register after a short delay (lets the server process
         # READY_FOR_CONNECTION) via a one-shot timer, so we don't block the
         # executor thread with a sleep.
+        # Destroying is safe only because brain_client_node is spun
+        # single-threaded, so this runs on the spin thread between callbacks.
         if self._reactivate_timer is not None:
             self._node.destroy_timer(self._reactivate_timer)
         self._reactivate_timer = self._node.create_timer(0.5, self._finish_reactivation)

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

@@ -19,6 +19,7 @@
 import queue
 import threading
 import time
+import traceback
 import uuid
 from typing import get_args
 
@@ -28,7 +29,7 @@
 from innate.skills import SkillCancelled, SkillFailed, use_invoker
 from rclpy.action import ActionClient, ActionServer, CancelResponse, GoalResponse
 from rclpy.callback_groups import ReentrantCallbackGroup
-from rclpy.executors import MultiThreadedExecutor
+from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor
 from rclpy.node import Node
 from std_msgs.msg import String
 from std_srvs.srv import Trigger
@@ -719,8 +720,15 @@ def main(args=None):
     executor.add_node(action_server)
     try:
         executor.spin()
-    except KeyboardInterrupt:
+    except (KeyboardInterrupt, ExternalShutdownException):
         pass
+    except Exception:
+        # An exception escaping spin() (e.g. InvalidHandle from an entity
+        # destroyed while the executor was using it) must not unwind past the
+        # teardown below: exiting with live zenoh entities panics rmw_zenoh's
+        # Rust runtime (SIGABRT). Log it and exit through the ordered teardown;
+        # launch respawns us either way, but from a clean exit.
+        action_server.get_logger().fatal(f"Executor spin crashed:\n{traceback.format_exc()}")
     action_server.destroy()
     # Guard against double-shutdown: avoids a teardown RCLError that exits 1.
     if rclpy.ok():

ros2_ws/src/brain/brain_client/brain_client/perception/camera.py

@@ -61,6 +61,11 @@ def start(self) -> None:
         self._head_sub = self._node.create_subscription(String, "/mars/head/current_position", self._on_head, 10)
 
     def stop(self) -> None:
+        # Destroying here is safe only because brain_client_node is spun
+        # single-threaded and stop() runs on that spin thread (between
+        # callbacks). On a multi-threaded executor this would race the wait
+        # set (InvalidHandle) — flag-gate the callbacks instead, like
+        # skills/robot_state.py.
         for sub in (self._image_sub, self._depth_sub, self._arm_sub, self._head_sub):
             if sub is not None:
                 self._node.destroy_subscription(sub)

ros2_ws/src/brain/brain_client/brain_client/perception/pose_tracking.py

@@ -68,6 +68,11 @@ def start(self) -> None:
         self._transform_timer = self._node.create_timer(1.0 / 30.0, self._fetch_transform)
 
     def stop(self) -> None:
+        # Destroying here is safe only because brain_client_node is spun
+        # single-threaded and stop() runs on that spin thread (between
+        # callbacks). On a multi-threaded executor this would race the wait
+        # set (InvalidHandle) — flag-gate the callbacks instead, like
+        # skills/robot_state.py.
         for sub in (self._odom_sub, self._nav_mode_sub):
             if sub is not None:
                 self._node.destroy_subscription(sub)

ros2_ws/src/brain/brain_client/brain_client/robot/manipulation.py

@@ -58,7 +58,11 @@ def __init__(self, node: Node, logger, lazy: bool = False):
         self._arm_state = None
         self._torque_enabled = None
 
-        # Subscription handles (created in start(), destroyed in stop())
+        # Subscription handles (created once in start(); kept for the node's
+        # lifetime — destroying one while the private executor thread spins
+        # races _take_subscription and crashes the process with InvalidHandle).
+        # stop() instead gates the callbacks via _active.
+        self._active = False
         self._ik_solution_sub = None
         self._ik_solution_fk_sub = None
         self._fk_pose_sub = None
@@ -87,7 +91,8 @@ def _spin_executor(self):
             self.logger.error(f"[ManipulationInterface] Executor stopped unexpectedly: {e}")
 
     def start(self):
-        """Create all subscriptions. Safe to call multiple times."""
+        """Enable arm-state feeds (subscriptions are created once). Safe to call multiple times."""
+        self._active = True
         if self._arm_state_sub is not None:
             return
         self._ik_solution_sub = self.node.create_subscription(
@@ -100,14 +105,12 @@ def start(self):
         self._arm_state_sub = self.node.create_subscription(JointState, "/mars/arm/state", self._arm_state_callback, 10)
 
     def stop(self):
-        """Destroy all subscriptions and clear cached state."""
-        for sub in (self._ik_solution_sub, self._ik_solution_fk_sub, self._fk_pose_sub, self._arm_state_sub):
-            if sub is not None:
-                self.node.destroy_subscription(sub)
-        self._ik_solution_sub = None
-        self._ik_solution_fk_sub = None
-        self._fk_pose_sub = None
-        self._arm_state_sub = None
+        """Deactivate arm-state feeds and clear cached state.
+
+        Subscriptions are deliberately kept alive (see __init__); the callbacks
+        early-return while inactive.
+        """
+        self._active = False
         self._ik_solution = None
         self._ik_solution_fk = None
         self._fk_pose = None

ros2_ws/src/brain/brain_client/brain_client/robot/mobility.py

@@ -40,30 +40,23 @@ def __init__(self, node: Node, logger, cmd_vel_topic: str = "/cmd_vel"):
         self.logger.info(f"MobilityInterface initialized with cmd_vel topic: {self.cmd_vel_topic}")
 
     def _schedule_stop(self, duration: float):
+        # One timer, created once and retargeted per command. Destroying a
+        # timer on a node a live executor is spinning races the executor's
+        # wait set (InvalidHandle -> crash), and creating a fresh one per
+        # deadman refresh (10-20/s) would leak cancelled timers in the node's
+        # timer list — reset/cancel on a single timer avoids both.
         if duration is None or duration <= 0.0:
             return
+        if self._stop_timer is None:
+            self._stop_timer = self.node.create_timer(duration, self._on_stop_timer)
+            return
+        self._stop_timer.timer_period_ns = int(duration * 1e9)
+        self._stop_timer.reset()
 
-        self._destroy_stop_timer()
-
-        def _stop_callback():
-            try:
-                stop_cmd = Twist()
-                self._cmd_vel_pub.publish(stop_cmd)
-                self.logger.debug("MobilityInterface: stop command published")
-            finally:
-                self._destroy_stop_timer()
-
-        self._stop_timer = self.node.create_timer(duration, _stop_callback)
-
-    def _destroy_stop_timer(self):
-        # destroy, don't just cancel: a cancelled timer stays in the node's
-        # timer list forever, which leaks at deadman rates (10-20 timers/s)
-        timer, self._stop_timer = self._stop_timer, None
-        if timer is not None:
-            try:
-                self.node.destroy_timer(timer)
-            except Exception:
-                pass
+    def _on_stop_timer(self):
+        self._stop_timer.cancel()  # one-shot: stays cancelled until the next _schedule_stop
+        self._cmd_vel_pub.publish(Twist())
+        self.logger.debug("MobilityInterface: stop command published")
 
     def send_cmd_vel(
         self,

ros2_ws/src/brain/brain_client/brain_client/skills/robot_state.py

@@ -45,6 +45,12 @@ def __init__(self, node, camera_node, *, manipulation, mobility, head, head_curr
         self._head_position_sub = None
         self._joint_states_sub = None
         self._battery_sub = None
+        # Feeds are gated by this flag instead of destroying the subscriptions:
+        # destroying a subscription the spinning MultiThreadedExecutor has
+        # already selected as "ready" races _take_subscription and crashes the
+        # process (InvalidHandle -> rmw_zenoh Rust panic -> SIGABRT), easiest
+        # to hit right after a skill cancellation.
+        self._active = False
         # warn once per missing state, not at 50 Hz while a slow topic
         # (battery publishes at 0.2 Hz) sends its first message
         self._warned_missing = set()
@@ -69,7 +75,10 @@ def inject_required_interfaces(self, skill) -> None:
 
     # --- subscriptions ---
     def start_subscriptions(self) -> None:
-        """Create robot-state subscriptions needed during skill execution."""
+        """Enable robot-state feeds for skill execution (subscriptions are created once)."""
+        self._warned_missing.clear()
+        self._manipulation.start()
+        self._active = True
         if self._odom_sub is not None:
             return
         self._odom_sub = self._node.create_subscription(Odometry, "/odom", self._on_odom, 10)
@@ -79,19 +88,15 @@ def start_subscriptions(self) -> None:
         )
         self._joint_states_sub = self._node.create_subscription(JointState, "/joint_states", self._on_joint_states, 10)
         self._battery_sub = self._node.create_subscription(BatteryState, "/battery_state", self._on_battery, 10)
-        self._warned_missing.clear()
-        self._manipulation.start()
 
     def stop_subscriptions(self) -> None:
-        """Destroy robot-state subscriptions when no skill is running."""
-        for sub in (self._odom_sub, self._map_sub, self._head_position_sub, self._joint_states_sub, self._battery_sub):
-            if sub is not None:
-                self._node.destroy_subscription(sub)
-        self._odom_sub = None
-        self._map_sub = None
-        self._head_position_sub = None
-        self._joint_states_sub = None
-        self._battery_sub = None
+        """Deactivate robot-state feeds when no skill is running.
+
+        The subscriptions are deliberately NOT destroyed (see __init__); the
+        callbacks early-return while inactive, so the idle cost is bounded by
+        message deserialization.
+        """
+        self._active = False
         self._manipulation.stop()
         self.last_odom = None
         self.last_map = None
@@ -100,18 +105,24 @@ def stop_subscriptions(self) -> None:
         self.last_battery = None
 
     def _on_odom(self, msg: Odometry) -> None:
-        self.last_odom = msg
+        if self._active:
+            self.last_odom = msg
 
     def _on_map(self, msg: OccupancyGrid) -> None:
-        self.last_map = msg
+        if self._active:
+            self.last_map = msg
 
     def _on_joint_states(self, msg: JointState) -> None:
-        self.last_joint_states = msg
+        if self._active:
+            self.last_joint_states = msg
 
     def _on_battery(self, msg: BatteryState) -> None:
-        self.last_battery = msg
+        if self._active:
+            self.last_battery = msg
 
     def _on_head_position(self, msg: String) -> None:
+        if not self._active:
+            return
         try:
             self.last_head_position = json.loads(msg.data)
         except Exception as e: