fix(brain): destroy retired skill instances' ROS entities on reload (navigator leak)

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

@@ -109,11 +109,22 @@ def __init__(self):
             head_current_position_topic=self.head_current_position_topic,
         )
 
+        # 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.
+        # 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_running = False
+        self._pending_retired_skills = []
+
         # Skill catalog (discovery, metadata, publishing, reload).
         self.catalog = SkillRepository(
             self,
             interface_injector=self.robot_state.inject_required_interfaces,
             simulator_mode=self.simulator_mode,
+            retire_instances=self._retire_skill_instances,
         )
 
         # Broadcasts every run's lifecycle (running/completed/failed/interrupted) so any
@@ -130,12 +141,6 @@ def __init__(self):
         self._behavior_goal_cancel_requested = set()
         self._behavior_goal_cancel_sent = set()
 
-        # 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.
-        self._skill_execution_lock = threading.Lock()
-        self._skill_running = False
-
         # ReentrantCallbackGroup so a cancel request can be serviced *while* a
         # skill's execute_callback is blocked waiting on the behavior result.
         # With the default MutuallyExclusiveCallbackGroup the cancel callback is
@@ -184,6 +189,20 @@ def __init__(self):
         # at a low rate so late-joining clients get it within one interval.
         self._skills_heartbeat_timer = self.create_timer(3.0, self.catalog.republish_cached)
 
+    # ================= retired skill instances =================
+    def _retire_skill_instances(self, instances):
+        """Dispose skill instances a reload replaced, deferring while a skill runs.
+
+        The running skill may itself be a retired instance (reload mid-run), and
+        its execute() is still spinning the ROS entities it owns — so disposal
+        waits for the run to end (drained in execute_callback's finally).
+        """
+        with self._skill_execution_lock:
+            if self._skill_running:
+                self._pending_retired_skills.extend(instances)
+                return
+        SkillRepository.dispose_instances(instances, self.get_logger())
+
     # ================= service handlers (delegate to catalog) =================
     def _handle_reload_skills(self, request, response):
         try:
@@ -328,6 +347,8 @@ def execute_callback(self, goal_handle):
         finally:
             with self._skill_execution_lock:
                 self._skill_running = False
+                retired, self._pending_retired_skills = self._pending_retired_skills, []
+            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)
         return result

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

@@ -39,11 +39,15 @@
 
 
 class SkillRepository:
-    def __init__(self, node, *, interface_injector, simulator_mode: bool):
+    def __init__(self, node, *, interface_injector, simulator_mode: bool, retire_instances=None):
         self._node = node
         self._logger = node.get_logger()
         self._inject = interface_injector
         self.simulator_mode = simulator_mode
+        # Called with the skill instances a reload replaced. The skills server
+        # passes a gate that defers disposal while a skill is executing; without
+        # a callback they are disposed inline.
+        self._retire_instances = retire_instances
 
         self.skill_loader = SkillLoader(self._logger)
         self._skills_directories = self._resolve_skills_directories()
@@ -100,6 +104,30 @@ def all_code_skills(self) -> list[tuple[str, tuple[str, Skill]]]:
         with self._skills_lock:
             return list(self._code_skills.items())
 
+    # --- retired-instance disposal ---
+    @staticmethod
+    def dispose_instances(instances: list[Skill], logger) -> None:
+        """shutdown() retired skill instances, logging (never raising) failures."""
+        for instance in instances:
+            try:
+                instance.shutdown()
+            except Exception as e:
+                logger.error(f"Error shutting down retired {type(instance).__name__} instance: {e}")
+
+    def _retire(self, instances: list[Skill]) -> None:
+        """Dispose instances a reload replaced (or hand them to the server's gate).
+
+        A replaced instance still holds live ROS entities (e.g. BasicNavigator
+        nodes). Left to the GC it is cyclic garbage: its graph entities persist
+        until an eventual gen-2 pass, so reloads leak subscriptions and memory.
+        """
+        if not instances:
+            return
+        if self._retire_instances is not None:
+            self._retire_instances(instances)
+        else:
+            self.dispose_instances(instances, self._logger)
+
     # --- watcher ---
     def start_watcher(self) -> None:
         self._hot_reload_watcher = HotReloadWatcher(
@@ -261,9 +289,11 @@ def reload_all(self) -> None:
         new_code_skills = self._load_code_skills(self._skills_directories)
         new_physical, new_in_training = self._load_physical_skills(self._skills_directories)
         with self._skills_lock:
+            old_code_skills = self._code_skills
             self._code_skills = new_code_skills
             self._physical_skills = new_physical
             self._in_training_skills = new_in_training
+        self._retire([instance for _name, instance in old_code_skills.values()])
         self._logger.info(f"Reloaded {len(new_code_skills)} code + {len(new_physical)} physical skills")
         self.publish_skills_list()
 
@@ -291,7 +321,10 @@ def reload_selective(self, skill_ids: list[str]) -> list[str]:
                     try:
                         instance = self._instantiate(cls, src_path)
                         with self._skills_lock:
+                            replaced = self._code_skills.get(skill_id)
                             self._code_skills[skill_id] = (display_name, instance)
+                        if replaced is not None:
+                            self._retire([replaced[1]])
                         reloaded.append(skill_id)
                         self._logger.info(f"Reloaded code skill: {skill_id}")
                     except Exception as e:
@@ -310,16 +343,18 @@ def _is_code_skill_id(self, skill_id: str) -> bool:
     def _prune_stale_skills(self) -> list[str]:
         """Drop catalog entries whose source file/directory is gone; returns removed ids."""
         removed = []
+        pruned_instances = []
         with self._skills_lock:
             for skill_id in list(self._code_skills):
                 if not self._code_source_exists(skill_id):
-                    del self._code_skills[skill_id]
+                    pruned_instances.append(self._code_skills.pop(skill_id)[1])
                     removed.append(skill_id)
             for skills in (self._physical_skills, self._in_training_skills):
                 for skill_id, data in list(skills.items()):
                     if not os.path.exists(os.path.join(data.get("directory", ""), "metadata.json")):
                         del skills[skill_id]
                         removed.append(skill_id)
+        self._retire(pruned_instances)
         if removed:
             self._logger.info(f"Pruned stale skills (source removed): {removed}")
         return removed

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

@@ -45,11 +45,23 @@ def _validate_class(self, skill_class: type[Skill]) -> bool:
     def _get_name(self, skill_class: type[Skill]) -> str:
         try:
             temp_logger = logging.getLogger(f"temp_{skill_class.__name__}")
-            return skill_class(temp_logger).name
+            instance = skill_class(temp_logger)
+            try:
+                return instance.name
+            finally:
+                # The throwaway instance may own ROS entities (e.g. BasicNavigator
+                # nodes); shut them down or every discovery pass leaks them.
+                self._shutdown_quietly(instance)
         except Exception as e:
             self.logger.debug(f"Could not get name from skill {skill_class.__name__}: {e}")
             return self._fallback_name(skill_class)
 
+    def _shutdown_quietly(self, instance: Skill) -> None:
+        try:
+            instance.shutdown()
+        except Exception as e:
+            self.logger.debug(f"Error shutting down temp {type(instance).__name__} instance: {e}")
+
     def reload_skill_by_file_stem(self, file_stem: str, directories: list[str]) -> tuple[type[Skill], Path] | None:
         """
         Reload a code skill by its file stem (e.g. 'navigate_to_position').

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

@@ -238,6 +238,18 @@ def cancel(self):
             return self.skills.cancel()
         return "Nothing to cancel"
 
+    def shutdown(self):  # noqa: B027
+        """Release resources this instance owns. Called when the server retires
+        it — reloads replace instances, and a retired one is never used again.
+
+        Override to destroy ROS nodes/entities the skill itself created (e.g.
+        Nav2 BasicNavigator nodes): a dropped instance is cyclic garbage whose
+        graph entities otherwise linger until an eventual gen-2 GC pass, so
+        every reload leaks subscriptions and memory. Leave entities created on
+        the shared server node alone — destroying entities under a spinning
+        executor is unsafe (see #497).
+        """
+
     @property
     def storage(self) -> SkillStorage:
         """Persistent per-skill key-value store (survives restarts), backed by

workspace/innate_skills/navigate_to_position.py

@@ -219,6 +219,22 @@ def cancel_navigation(self):
         # Set the cancellation flag
         self._cancel_requested.set()
 
+    def destroy(self):
+        """Destroy the navigator nodes so their graph entities disappear now,
+        not at some eventual GC pass. Safe outside go_to_position — nothing
+        else spins these nodes."""
+        for navigator in (self.navigator, self.navigator_mapfree, self.navigator_navigation):
+            try:
+                # Humble's BasicNavigator.destroy_node() misses this client; its
+                # live handle would keep the rcl node and graph entities alive.
+                navigator.assisted_teleop_client.destroy()
+            except Exception as e:
+                self.logger.warning(f"Error destroying assisted_teleop client: {e}")
+            try:
+                navigator.destroy_node()
+            except Exception as e:
+                self.logger.warning(f"Error destroying navigator node: {e}")
+
 
 class NavigateToPosition(Skill):
     def __init__(self, logger):
@@ -265,3 +281,7 @@ def cancel(self):
         self.logger.debug("Canceling navigation task")
         self.nav2_controller.cancel_navigation()
         return "Navigation canceled"
+
+    def shutdown(self):
+        """Destroy this instance's navigator nodes when the server retires it."""
+        self.nav2_controller.destroy()

workspace/innate_skills/navigate_to_position_sim.py

@@ -135,6 +135,24 @@ def cancel_navigation(self):
         self.logger.debug("Canceling current navigation task...")
         self._cancel_requested.set()
 
+    def destroy(self):
+        """Destroy the navigator and publisher nodes so their graph entities
+        disappear now, not at some eventual GC pass."""
+        try:
+            # Humble's BasicNavigator.destroy_node() misses this client; its
+            # live handle would keep the rcl node and graph entities alive.
+            self.navigator.assisted_teleop_client.destroy()
+        except Exception as e:
+            self.logger.warning(f"Error destroying assisted_teleop client: {e}")
+        try:
+            self.navigator.destroy_node()
+        except Exception as e:
+            self.logger.warning(f"Error destroying navigator node: {e}")
+        try:
+            self.node.destroy_node()
+        except Exception as e:
+            self.logger.warning(f"Error destroying path planning node: {e}")
+
 
 class NavigateToPositionSim(Skill):
     def __init__(self, logger):
@@ -180,3 +198,7 @@ def cancel(self):
         self.logger.debug("Canceling navigation task")
         self.path_controller.cancel_navigation()
         return "Navigation canceled"
+
+    def shutdown(self):
+        """Destroy this instance's ROS nodes when the server retires it."""
+        self.path_controller.destroy()