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 af12d3661..73762bcb0 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 @@ -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 diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/catalog.py b/ros2_ws/src/brain/brain_client/brain_client/skills/catalog.py index 203e02ee2..f7c2b674f 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/catalog.py +++ b/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 diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/loader.py b/ros2_ws/src/brain/brain_client/brain_client/skills/loader.py index 9ee1442d7..2a972b611 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/loader.py +++ b/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'). diff --git a/ros2_ws/src/brain/brain_client/brain_client/skills/types.py b/ros2_ws/src/brain/brain_client/brain_client/skills/types.py index ea77d2b39..e1069accd 100644 --- a/ros2_ws/src/brain/brain_client/brain_client/skills/types.py +++ b/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 diff --git a/workspace/innate_skills/navigate_to_position.py b/workspace/innate_skills/navigate_to_position.py index a5e3ed832..6c0ecf12c 100644 --- a/workspace/innate_skills/navigate_to_position.py +++ b/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() diff --git a/workspace/innate_skills/navigate_to_position_sim.py b/workspace/innate_skills/navigate_to_position_sim.py index fd73f4ca2..cda712b42 100644 --- a/workspace/innate_skills/navigate_to_position_sim.py +++ b/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()