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

@@ -42,10 +42,14 @@ def __init__(self, node: Node, logger, lazy: bool = False):
         """
         self.node = rclpy.create_node(f"{node.get_name()}_manipulation_interface")
         self.logger = logger
-        self._executor = rclpy.executors.SingleThreadedExecutor()
-        self._executor.add_node(self.node)
-        self._executor_thread = threading.Thread(target=self._spin_executor, daemon=True)
-        self._executor_thread.start()
+        # The private executor spins only while a skill is active
+        # (start()..stop()). While parked, messages arriving on the
+        # always-alive subscriptions just rotate in the bounded rmw queues at
+        # no Python cost — dispatching them through an executor costs ~half a
+        # Jetson core at the ~600 msgs/s these feeds add up to.
+        self._executor = None
+        self._executor_thread = None
+        self._lifecycle_lock = threading.Lock()
         self._ik_lock = threading.Lock()
 
         # Publishers
@@ -58,7 +62,12 @@ 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 and parks the
+        # executor.
+        self._active = False
         self._ik_solution_sub = None
         self._ik_solution_fk_sub = None
         self._fk_pose_sub = None
@@ -80,34 +89,51 @@ def __init__(self, node: Node, logger, lazy: bool = False):
 
         self.logger.info("ManipulationInterface initialized")
 
-    def _spin_executor(self):
+    def _spin_executor(self, executor):
         try:
-            self._executor.spin()
+            executor.spin()
         except Exception as e:
             self.logger.error(f"[ManipulationInterface] Executor stopped unexpectedly: {e}")
 
     def start(self):
-        """Create all subscriptions. Safe to call multiple times."""
-        if self._arm_state_sub is not None:
-            return
-        self._ik_solution_sub = self.node.create_subscription(
-            JointState, "/ik_solution", self._ik_solution_callback, 10
-        )
-        self._ik_solution_fk_sub = self.node.create_subscription(
-            PoseStamped, "/ik_solution_fk", self._ik_solution_fk_callback, 10
-        )
-        self._fk_pose_sub = self.node.create_subscription(PoseStamped, "/fk_pose", self._fk_pose_callback, 10)
-        self._arm_state_sub = self.node.create_subscription(JointState, "/mars/arm/state", self._arm_state_callback, 10)
+        """Enable arm-state feeds: spin up the private executor and create the
+        subscriptions once. Safe to call multiple times."""
+        with self._lifecycle_lock:
+            self._active = True
+            if self._executor is None:
+                self._executor = rclpy.executors.SingleThreadedExecutor()
+                self._executor.add_node(self.node)
+                self._executor_thread = threading.Thread(
+                    target=self._spin_executor, args=(self._executor,), daemon=True
+                )
+                self._executor_thread.start()
+            if self._arm_state_sub is not None:
+                return
+            self._ik_solution_sub = self.node.create_subscription(
+                JointState, "/ik_solution", self._ik_solution_callback, 10
+            )
+            self._ik_solution_fk_sub = self.node.create_subscription(
+                PoseStamped, "/ik_solution_fk", self._ik_solution_fk_callback, 10
+            )
+            self._fk_pose_sub = self.node.create_subscription(PoseStamped, "/fk_pose", self._fk_pose_callback, 10)
+            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, park the private executor and clear cached state.
+
+        Subscriptions are deliberately kept alive (see __init__); the callbacks
+        early-return while inactive, and with the executor parked they are not
+        invoked at all between skills.
+        """
+        with self._lifecycle_lock:
+            self._active = False
+            if self._executor is not None:
+                self._executor.shutdown()
+                self._executor_thread.join(timeout=2.0)
+                self._executor = None
+                self._executor_thread = None
         self._ik_solution = None
         self._ik_solution_fk = None
         self._fk_pose = None
@@ -116,8 +142,6 @@ def stop(self):
     def shutdown(self):
         """Stop the manipulation helper node and its private executor."""
         self.stop()
-        self._executor.shutdown()
-        self._executor_thread.join(timeout=2.0)
         self.node.destroy_node()
 
     def spin_node_to_refresh_topics(self, count: int = 10, timeout_sec: float = 0.001):
@@ -136,20 +160,24 @@ def _wait_for_future(self, future, timeout_sec: float | None = None) -> bool:
 
     def _ik_solution_callback(self, msg: JointState):
         """Store the latest IK solution."""
-        self.logger.debug(f"IK solution received: {msg}")
-        self._ik_solution = msg
+        if self._active:
+            self.logger.debug(f"IK solution received: {msg}")
+            self._ik_solution = msg
 
     def _ik_solution_fk_callback(self, msg: PoseStamped):
         """Store the FK of the latest IK solution (what commanded joints map to)."""
-        self._ik_solution_fk = msg
+        if self._active:
+            self._ik_solution_fk = msg
 
     def _fk_pose_callback(self, msg: PoseStamped):
         """Store the latest FK pose."""
-        self._fk_pose = msg
+        if self._active:
+            self._fk_pose = msg
 
     def _arm_state_callback(self, msg: JointState):
         """Store the latest arm state (includes effort/load)."""
-        self._arm_state = msg
+        if self._active:
+            self._arm_state = msg
 
     def get_current_end_effector_pose(self) -> dict | 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,
@@ -90,6 +83,10 @@ def send_cmd_vel(
 
         if duration is not None and duration > 0.0:
             self._schedule_stop(duration)
+        elif self._stop_timer is not None:
+            # Continuous motion requested: disarm any stop still pending from
+            # an earlier timed command so it doesn't cut this one short.
+            self._stop_timer.cancel()
 
     def rotate_in_place(self, angular_speed: float, duration: float) -> None:
         """Rotate in place with specified angular speed for a duration (non-blocking).

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

@@ -45,6 +45,16 @@ 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 an executor 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. The subscriptions live on the
+        # manipulation interface's private node, whose executor is parked
+        # between skills — always-alive high-rate feeds (/joint_states + head
+        # position alone are ~400 msgs/s) would otherwise cost ~half a Jetson
+        # core in executor dispatch even while idle.
+        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,29 +79,31 @@ 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()
+        # start() first: it spins up the private executor that also dispatches
+        # the robot-state subscriptions below
+        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)
-        self._map_sub = self._node.create_subscription(OccupancyGrid, "/map", self._on_map, 1)
-        self._head_position_sub = self._node.create_subscription(
+        feed_node = self._manipulation.node
+        self._odom_sub = feed_node.create_subscription(Odometry, "/odom", self._on_odom, 10)
+        self._map_sub = feed_node.create_subscription(OccupancyGrid, "/map", self._on_map, 1)
+        self._head_position_sub = feed_node.create_subscription(
             String, self._head_current_position_topic, self._on_head_position, 10
         )
-        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()
+        self._joint_states_sub = feed_node.create_subscription(JointState, "/joint_states", self._on_joint_states, 10)
+        self._battery_sub = feed_node.create_subscription(BatteryState, "/battery_state", self._on_battery, 10)
 
     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, and parking the private
+        executor (manipulation.stop()) stops them being invoked at all.
+        """
+        self._active = False
         self._manipulation.stop()
         self.last_odom = None
         self.last_map = None
@@ -100,18 +112,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: