diff --git a/ros2_ws/src/brain/brain_client/brain_client/core/lifecycle.py b/ros2_ws/src/brain/brain_client/brain_client/core/lifecycle.py index 743d860ee..b36321aa0 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/core/lifecycle.py +++ b/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) 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 0166f7951..af12d3661 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 @@ -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(): diff --git a/ros2_ws/src/brain/brain_client/brain_client/perception/camera.py b/ros2_ws/src/brain/brain_client/brain_client/perception/camera.py index d97ee0363..31d54b75b 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/perception/camera.py +++ b/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) diff --git a/ros2_ws/src/brain/brain_client/brain_client/perception/pose_tracking.py b/ros2_ws/src/brain/brain_client/brain_client/perception/pose_tracking.py index 88b016b57..dce19af71 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/perception/pose_tracking.py +++ b/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) diff --git a/ros2_ws/src/brain/brain_client/brain_client/robot/manipulation.py b/ros2_ws/src/brain/brain_client/brain_client/robot/manipulation.py index 18715b2b9..fd266dc9a 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/robot/manipulation.py +++ b/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: """ diff --git a/ros2_ws/src/brain/brain_client/brain_client/robot/mobility.py b/ros2_ws/src/brain/brain_client/brain_client/robot/mobility.py index 3d8f6d2a4..c2bc99642 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/robot/mobility.py +++ b/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). diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/robot_state.py b/ros2_ws/src/brain/brain_client/brain_client/skills/robot_state.py index 1686521a0..bc1b6814e 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/robot_state.py +++ b/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: