diff --git a/robotics_application_manager/manager/agent_group.py b/robotics_application_manager/manager/agent_group.py new file mode 100644 index 0000000..88f9590 --- /dev/null +++ b/robotics_application_manager/manager/agent_group.py @@ -0,0 +1,116 @@ +"""Agent group abstraction for the Robotics Application Manager. + +RAM's lifecycle operations (play / pause / resume / reset / stop) used to act on +a single application process. Multi-agent exercises (e.g. drone cat-and-mouse, +which runs a "cat" and a "mouse") need them to act on *all* the agents at once. + +Rather than teaching the RAM FiniteStateMachine about multiple agents, the FSM +stays single-target: it fires one transition, and that transition fans the +operation out over every member of this group. So a single ``pause`` trigger +becomes "suspend agent0, suspend agent1, ... ", a single ``reset`` becomes a +sequence over each agent, and so on. The FSM definition never changes; only the +execution layer behind each handler iterates the group. + +This holds N agents (1 for most exercises, 2 for cat-mouse, more in future +multi-robot exercises) and exposes the per-agent *process* operations. World / +simulator level calls (pause_sim, reset_sim) remain single calls in the manager, +because a shared Gazebo world has one physics clock and cannot be paused +per-agent. +""" + +import os +import signal + +import psutil + +from robotics_application_manager.libs import stop_process_and_children +from robotics_application_manager.ram_logging import LogManager + + +class AgentGroup: + """A set of agent processes managed as one unit. + + Each lifecycle method fans the operation out over every member, so callers + treat the whole group as if it were a single agent. + """ + + def __init__(self): + # name -> subprocess.Popen + self._agents = {} + + # ----- membership ------------------------------------------------------- + + def add(self, name, proc): + """Register an agent process under a name (e.g. 'agentA', 'agentB').""" + self._agents[name] = proc + + def clear(self): + """Forget all members without touching the processes.""" + self._agents = {} + + def names(self): + return list(self._agents.keys()) + + def __len__(self): + return len(self._agents) + + def __bool__(self): + # Lets callers keep writing `if self.application_processes:` + return bool(self._agents) + + def __iter__(self): + return iter(self._agents.values()) + + # ----- group lifecycle operations (each fans out over all members) ------ + + def pause_all(self): + """Suspend every agent process and its whole child tree (SIGSTOP).""" + for proc in self._agents.values(): + self._apply_to_tree(proc, lambda child: child.suspend()) + + def resume_all(self): + """Resume every agent process and its whole child tree (SIGCONT).""" + for proc in self._agents.values(): + self._apply_to_tree(proc, lambda child: child.resume()) + + def signal_stop_all(self): + """SIGSTOP each top-level agent process (used to gate them before + the simulator unpauses, so no agent acts on a still-paused world).""" + self._signal_all(signal.SIGSTOP) + + def signal_cont_all(self): + """SIGCONT each top-level agent process (release after unpause).""" + self._signal_all(signal.SIGCONT) + + def kill_all(self): + """Terminate every agent process and its children, then empty the group.""" + for name, proc in self._agents.items(): + try: + stop_process_and_children(proc) + except Exception: + LogManager.logger.exception(f"Error stopping agent '{name}'") + self.clear() + + # ----- helpers ---------------------------------------------------------- + + def _signal_all(self, sig): + for proc in self._agents.values(): + try: + os.kill(proc.pid, sig) + except ProcessLookupError: + pass + + @staticmethod + def _apply_to_tree(proc, fn): + """Run ``fn`` on the process and every descendant, tolerating races.""" + try: + parent = psutil.Process(proc.pid) + tree = parent.children(recursive=True) + tree.append(parent) + for child in tree: + try: + fn(child) + except psutil.NoSuchProcess: + pass + except psutil.NoSuchProcess: + pass diff --git a/robotics_application_manager/manager/launcher/launcher_gzsim.py b/robotics_application_manager/manager/launcher/launcher_gzsim.py index 3959531..c91cc81 100644 --- a/robotics_application_manager/manager/launcher/launcher_gzsim.py +++ b/robotics_application_manager/manager/launcher/launcher_gzsim.py @@ -103,7 +103,7 @@ def unpause(self): 10000, ) - def reset(self, robot_entity=None): + def reset(self, robot_entities=None): node = Node() node.request( @@ -114,14 +114,20 @@ def reset(self, robot_entity=None): 10000, ) - if robot_entity is not None: - node.request( - f"/world/default/remove", - Entity(name=robot_entity, type=Entity.MODEL), - Entity, - Boolean, - 5000, - ) + # remove each robot entity before resetting the world (a world reset on + # its own doesn't clear runtime-spawned models). accepts a single name + # or a list of them, so one robot and N robots take the same path. + if robot_entities: + if isinstance(robot_entities, str): + robot_entities = [robot_entities] + for entity in robot_entities: + node.request( + f"/world/default/remove", + Entity(name=entity, type=Entity.MODEL), + Entity, + Boolean, + 5000, + ) node.request( f"/world/default/control", diff --git a/robotics_application_manager/manager/launcher/launcher_robot.py b/robotics_application_manager/manager/launcher/launcher_robot.py index 13f93fd..5f92088 100644 --- a/robotics_application_manager/manager/launcher/launcher_robot.py +++ b/robotics_application_manager/manager/launcher/launcher_robot.py @@ -1,5 +1,6 @@ """LauncherRobot module for managing robot launchers in different simulation worlds.""" +import re from typing import Optional from pydantic import BaseModel @@ -37,8 +38,55 @@ class LauncherRobot(BaseModel): entity: str = "" start_pose: Optional[list] = [] + @staticmethod + def make_names_unique(robot_cfgs): + """Rename only the robots whose names clash with an earlier one. + + The first robot to use a name keeps it; a later robot asking for the same + name gets a numbered suffix: + + car, vehicle, car -> car, vehicle, car_1 + + Entity and namespace are handled separately: a robot can have one and not + the other, and a clash in one does not imply a clash in the other. Entity + is a normal field, but namespace is not: it sits inside the extra_config + string (e.g. "sensor:=camera namespace:=drone"), so we have to dig it out + with a regex and write it back the same way. + """ + for key in ("entity", "namespace"): + used = set() + for robot_cfg in robot_cfgs: + if key == "entity": + name = robot_cfg.get("entity") + else: + match = re.search( + r"namespace:=(\S+)", robot_cfg.get("extra_config", "") + ) + name = match.group(1) if match else None + + if not name: + continue + if name not in used: + used.add(name) + continue + + index = 1 + while f"{name}_{index}" in used: + index += 1 + new_name = f"{name}_{index}" + + if key == "entity": + robot_cfg["entity"] = new_name + else: + robot_cfg["extra_config"] = re.sub( + r"namespace:=\S+", + f"namespace:={new_name}", + robot_cfg["extra_config"], + ) + used.add(new_name) + def run(self, entity="", start_pose=None, extra_config=None): - """Run the robot launcher with an optional start pose.""" + """Start the robot launcher. Does not wait for the robot to spawn.""" self.entity = entity if start_pose is not None: @@ -53,6 +101,26 @@ def run(self, entity="", start_pose=None, extra_config=None): self.launchers.append(launcher) LogManager.logger.info(self.launchers) + @staticmethod + def wait_for(robot_launchers): + """Wait until every robot in the list has spawned. + + Collects the entities of all the robots and checks them in a single + poll loop, instead of waiting for one robot at a time. Entries with no + launcher (a world with no robot assigned) are skipped. + """ + launchers = [ + launcher + for robot_launcher in robot_launchers + if robot_launcher is not None + for launcher in robot_launcher.launchers + ] + if not launchers: + return + # They are all the same simulator launcher, so any of them can run the + # poll for the whole list of entities. + launchers[0].wait([launcher.entity for launcher in launchers]) + def terminate(self): """Terminate all robot launchers and clear the launchers list.""" LogManager.logger.info("Terminating robots launchers") diff --git a/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py b/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py index f47eea8..85afb92 100644 --- a/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py +++ b/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py @@ -13,8 +13,8 @@ import logging from robotics_application_manager import LogManager -from gz.transport13 import Node +from gz.transport13 import Node from gz.msgs10.empty_pb2 import Empty from gz.msgs10.scene_pb2 import Scene @@ -24,43 +24,57 @@ class LauncherRobotRos2Api(ILauncher): module: str launch_file: str threads: List[Any] = [] + entity: str = "" def run(self, entity, robot_pose, extra_config, callback): + """Start the robot's launch. Does not wait for it to spawn. + + Only fires the ros2 launch (async, in its own process) and returns, so + the manager can start every robot's launch first and then wait for them + all together (see wait()). That way N robots spawn in parallel (~one + spawn time, not N), which also makes every reset faster. + """ DRI_PATH = self.get_dri_path() ACCELERATION_ENABLED = self.check_device(DRI_PATH) logging.getLogger("roslaunch").setLevel(logging.CRITICAL) + self.entity = entity x, y, z, R, P, Y = robot_pose if extra_config == "None": extra_config = "" + # Pass the entity name to the launch. Entity is the gazebo model name + # (used to spawn and remove the model). if ACCELERATION_ENABLED: - exercise_launch_cmd = f"export VGL_DISPLAY={DRI_PATH}; vglrun ros2 launch {self.launch_file} x:={x} y:={y} z:={z} R:={R} P:={P} Y:={Y} {extra_config}" + exercise_launch_cmd = f"export VGL_DISPLAY={DRI_PATH}; vglrun ros2 launch {self.launch_file} x:={x} y:={y} z:={z} R:={R} P:={P} Y:={Y} entity:={entity} {extra_config}" else: - exercise_launch_cmd = f"ros2 launch {self.launch_file} x:={x} y:={y} z:={z} R:={R} P:={P} Y:={Y} {extra_config}" + exercise_launch_cmd = f"ros2 launch {self.launch_file} x:={x} y:={y} z:={z} R:={R} P:={P} Y:={Y} entity:={entity} {extra_config}" exercise_launch_thread = DockerThread(exercise_launch_cmd) exercise_launch_thread.start() self.threads.append(exercise_launch_thread) - # Wait until robot entity has spawned + def wait(self, entities, timeout=90): + """Block until every entity in the list has appeared in the gazebo scene. + + One poll loop for the whole list, not one per robot: every launch was + started first, so the robots spawn in parallel and each one is ticked + off as it shows up. gz Node.request blocks, so this runs on the main + thread (polling it from worker threads starves the GIL). + """ + pending = set(entities) node = Node() - spawned = False - while not spawned: - a = node.request( - f"/world/default/scene/info", - Empty(), - Empty, - Scene, - 1000, + start = time.time() + while pending and time.time() - start < timeout: + ok, scene = node.request( + "/world/default/scene/info", Empty(), Empty, Scene, 1000 ) - if a[0]: - for model in a[1].model: - if model.name == entity: - spawned = True - LogManager.logger.info("Robot spawned OK") + if ok: + pending -= {model.name for model in scene.model} + if pending: + LogManager.logger.error(f"Robots did not spawn in time: {pending}") def terminate(self): LogManager.logger.info(f"Terminating robot launcher") diff --git a/robotics_application_manager/manager/launcher/launcher_tools.py b/robotics_application_manager/manager/launcher/launcher_tools.py index a294de1..32f6778 100644 --- a/robotics_application_manager/manager/launcher/launcher_tools.py +++ b/robotics_application_manager/manager/launcher/launcher_tools.py @@ -121,9 +121,9 @@ def unpause(self): for launcher in self.launchers: launcher.unpause() - def reset(self, robot_entity=None): + def reset(self, robot_entities=None): for launcher in self.launchers: - launcher.reset(robot_entity) + launcher.reset(robot_entities) def pass_msg(self, data): for launcher in self.launchers: diff --git a/robotics_application_manager/manager/manager.py b/robotics_application_manager/manager/manager.py index 770e4e3..80b21d3 100644 --- a/robotics_application_manager/manager/manager.py +++ b/robotics_application_manager/manager/manager.py @@ -15,8 +15,6 @@ import os import signal import subprocess -import re -import psutil import shutil import time import base64 @@ -34,7 +32,6 @@ from robotics_application_manager.libs import ( check_gpu_acceleration, get_class_from_file, - stop_process_and_children, ConfigurationManager, ) from robotics_application_manager.ram_logging import LogManager @@ -45,6 +42,7 @@ ) from robotics_application_manager.manager.lint import Lint from robotics_application_manager.manager.editor import serialize_completions +from robotics_application_manager.manager.agent_group import AgentGroup class Manager: @@ -224,10 +222,13 @@ def __init__(self, host: str, port: int): self.consumer = ManagerConsumer(host, port, self.queue) self.scene_launcher = None self.world_type = None - self.robot_launcher = None - self.robot_config = None + self.robot_launchers = [] + self.robot_configs = [] self.tools_launcher = None - self.application_process = None + # Group of agent processes (1 for single-agent exercises, 2+ for + # multi-agent ones like drone cat-mouse). Lifecycle ops fan out over + # all members; the FSM stays single-target. See AgentGroup. + self.application_processes = AgentGroup() self.running = True self.linter = Lint() @@ -323,11 +324,9 @@ def on_launch_world(self, event): """ cfg_dict = event.kwargs.get("data", {}) scene_cfg = cfg_dict["scene"] - robot_cfg = cfg_dict["robot"] - - # Backwards compatibility for now - if isinstance(robot_cfg, list): - robot_cfg = robot_cfg[0] + # 'robot' is a list of robot configs, one entry per robot + robot_cfgs = cfg_dict["robot"] + LauncherRobot.make_names_unique(robot_cfgs) # Launch scene try: @@ -351,25 +350,37 @@ def on_launch_world(self, event): self.scene_launcher = LauncherScene(**cfg.model_dump()) LogManager.logger.info(str(self.scene_launcher)) - # Launch robot - self.robot_launcher = None - if robot_cfg["type"] is not None: + # Launch robots. A world with no robot assigned still sends a config, but + # a dummy one: it has no type, so it gets no launcher. Those entries keep + # a None launcher, so the two lists stay index-aligned and the callers + # just skip them. + self.robot_configs = robot_cfgs + self.robot_launchers = [] + for robot_cfg in robot_cfgs: + if robot_cfg["type"] is None: + self.robot_launchers.append(None) + continue try: cfg = ConfigurationManager.validate(robot_cfg) LogManager.logger.info("Launching robot from the RB") LogManager.logger.info(cfg) except ValueError as e: LogManager.logger.error(f"Configuration validation failed: {e}") - - self.robot_launcher = LauncherRobot(**cfg.model_dump()) - self.robot_config = robot_cfg - LogManager.logger.info(str(self.robot_launcher)) + self.robot_launchers.append(None) + continue + self.robot_launchers.append(LauncherRobot(**cfg.model_dump())) + LogManager.logger.info(str(self.robot_launchers[-1])) self.scene_launcher.run() - if self.robot_launcher is not None: - self.robot_launcher.run( - robot_cfg["entity"], robot_cfg["start_pose"], robot_cfg["extra_config"] + for launcher, robot_cfg in zip(self.robot_launchers, self.robot_configs): + if launcher is None: + continue + launcher.run( + robot_cfg["entity"], + robot_cfg["start_pose"], + robot_cfg["extra_config"], ) + LauncherRobot.wait_for(self.robot_launchers) LogManager.logger.info("Launch transition finished") def prepare_custom_world(self, cfg_dict): @@ -728,6 +739,22 @@ def on_change_style(self, event): except Exception as e: LogManager.logger.exception(f"Error refreshing GTK applications: {e}") + @staticmethod + def _agent_name(entrypoint, code_dir="/workspace/code"): + """Name an agent after the folder its entrypoint lives in. + + Each extra agent ships in its own folder, so the folder name is what + tells them apart. An exercise naming those folders after its robots + gets agents called drone0 / drone1 instead of anonymous ones. The + student's entrypoint sits at the root of the code, so it falls back to + the file name. + """ + relative_path = os.path.relpath(entrypoint, code_dir) + folder = os.path.dirname(relative_path) + if folder: + return folder.replace(os.sep, "_") + return os.path.splitext(os.path.basename(relative_path))[0] + def on_run_application(self, event): """ Handle the 'run_application' event. @@ -740,12 +767,7 @@ def on_run_application(self, event): event: The event object containing application configuration and code data. """ # Kill already running code - try: - proc = psutil.Process(self.application_process.pid) - proc.suspend() - proc.kill() - except Exception: - pass + self.application_processes.kill_all() # Delete old files if os.path.exists("/workspace/code"): @@ -754,11 +776,11 @@ def on_run_application(self, event): # Extract app config app_cfg = event.kwargs.get("data", {}) - entrypoint = app_cfg["entrypoint"] - - # Backwards compatibility for now - if isinstance(entrypoint, list): - entrypoint = entrypoint[0] + + entrypoints = app_cfg["entrypoint"] + if not isinstance(entrypoints, list): + entrypoints = [entrypoints] + entrypoint = entrypoints[0] to_lint = app_cfg["linter"] @@ -767,6 +789,7 @@ def on_run_application(self, event): _, _, code = app_cfg["code"].partition("base64,") with open("/workspace/code/app.zip", "wb") as result: result.write(base64.b64decode(code)) + zip_ref = zipfile.ZipFile("/workspace/code/app.zip", "r") zip_ref.extractall("/workspace/code") zip_ref.close() @@ -797,9 +820,8 @@ def on_run_application(self, event): if returncode != 0: raise Exception("Failed to compile") - self.unpause_sim() if entrypoint.endswith(".launch.py"): - self.application_process = subprocess.Popen( + proc = subprocess.Popen( [ f"source /workspace/code/install/setup.bash && ros2 launch {entrypoint}" ], @@ -813,8 +835,7 @@ def on_run_application(self, event): start_new_session=True, ) else: - - self.application_process = subprocess.Popen( + proc = subprocess.Popen( [ "source /workspace/code/install/setup.bash && ros2 run academy academyCode" ], @@ -827,6 +848,8 @@ def on_run_application(self, event): executable="/bin/bash", start_new_session=True, ) + self.application_processes.add("agentA", proc) + self.unpause_sim() return # Pass the linter @@ -844,15 +867,31 @@ def on_run_application(self, event): fds = os.listdir("/dev/pts/") console_fd = str(max(map(int, fds[:-1]))) - self.unpause_sim() - self.application_process = subprocess.Popen( - ["python3", entrypoint], - stdin=open("/dev/pts/" + console_fd, "r"), - stdout=open("/dev/pts/" + console_fd, "w"), - stderr=sys.stdout, - bufsize=1024, - universal_newlines=True, - ) + agent_env = os.environ.copy() + agent_env["PYTHONPATH"] = "/workspace/code:" + agent_env.get("PYTHONPATH", "") + + for agent_entrypoint in entrypoints: + if not os.path.isfile(agent_entrypoint): + continue + proc = subprocess.Popen( + ["python3", agent_entrypoint], + env=agent_env, + stdin=open("/dev/pts/" + console_fd, "r"), + stdout=open("/dev/pts/" + console_fd, "w"), + stderr=sys.stdout, + bufsize=1024, + universal_newlines=True, + ) + self.application_processes.add( + self._agent_name(agent_entrypoint), proc + ) + + # SIGSTOP every agent first, then unpause gazebo, then SIGCONT them all + self.application_processes.signal_stop_all() + try: + self.unpause_sim() + finally: + self.application_processes.signal_cont_all() LogManager.logger.info("Run application transition finished") @@ -866,11 +905,10 @@ def on_terminate_application(self, event): Parameters: event: The event object associated with the termination request. """ - if self.application_process: + if self.application_processes: try: - stop_process_and_children(self.application_process) - self.application_process = None self.pause_sim() + self.application_processes.kill_all() self.reset_sim() except Exception: LogManager.logger.exception("No application running") @@ -895,9 +933,11 @@ def on_terminate_world(self, event): self.scene_launcher.terminate() self.scene_launcher = None self.world_type = None - if self.robot_launcher is not None: - self.robot_launcher.terminate() - self.robot_launcher = None + for robot_launcher in self.robot_launchers: + if robot_launcher is not None: + robot_launcher.terminate() + self.robot_launchers = [] + self.robot_configs = [] def on_disconnect(self, event): """ @@ -907,10 +947,9 @@ def on_disconnect(self, event): terminates launchers, and restarts the script. """ - if self.application_process: + if self.application_processes: try: - stop_process_and_children(self.application_process) - self.application_process = None + self.application_processes.kill_all() except Exception as e: LogManager.logger.exception("Exception stopping application process") @@ -920,9 +959,11 @@ def on_disconnect(self, event): except Exception as e: LogManager.logger.exception("Exception terminating tools launcher") - if self.robot_launcher: + for robot_launcher in self.robot_launchers: + if robot_launcher is None: + continue try: - self.robot_launcher.terminate() + robot_launcher.terminate() except Exception as e: LogManager.logger.exception("Exception terminating robot launcher") @@ -943,15 +984,9 @@ def process_message(self, message): self.consumer.send_message(message.response(response)) def on_pause(self, msg): - if self.application_process is not None: - proc = psutil.Process(self.application_process.pid) - children = proc.children(recursive=True) - children.append(proc) - for p in children: - try: - p.suspend() - except psutil.NoSuchProcess: - pass + if self.application_processes: + # Suspend every agent in the group, then freeze the shared world. + self.application_processes.pause_all() self.pause_sim() else: LogManager.logger.warning( @@ -967,15 +1002,9 @@ def on_resume(self, msg): Parameters: msg: The event or message triggering the resume action. """ - if self.application_process is not None: - proc = psutil.Process(self.application_process.pid) - children = proc.children(recursive=True) - children.append(proc) - for p in children: - try: - p.resume() - except psutil.NoSuchProcess: - pass + if self.application_processes: + # Unfreeze the shared world, then resume every agent in the group. + self.application_processes.resume_all() self.unpause_sim() else: LogManager.logger.warning( @@ -1005,27 +1034,37 @@ def reset_sim(self): the appropriate ROS or Gazebo services based on the visualization type, and relaunches the robot if a launcher is available. """ - if self.robot_launcher: - self.robot_launcher.terminate() + # kill every robot launcher first (so we're not resetting a live robot) + for robot_launcher in self.robot_launchers: + if robot_launcher is not None: + robot_launcher.terminate() + + if self.tools_launcher is None: + return try: - entity = None - if self.robot_config is not None: - entity = self.robot_config["entity"] - self.tools_launcher.reset(entity) + entities = [ + cfg["entity"] + for launcher, cfg in zip(self.robot_launchers, self.robot_configs) + if launcher is not None + ] + self.tools_launcher.reset(entities) except subprocess.TimeoutExpired as e: self.write_to_tool_terminal(f"{e}\n\n") raise Exception("Failed to reset simulator") - if self.robot_launcher: - try: - self.robot_launcher.run( - self.robot_config["entity"], - self.robot_config["start_pose"], - self.robot_config["extra_config"], + try: + for launcher, robot_cfg in zip(self.robot_launchers, self.robot_configs): + if launcher is None: + continue + launcher.run( + robot_cfg["entity"], + robot_cfg["start_pose"], + robot_cfg["extra_config"], ) - except Exception as e: - LogManager.logger.exception("Exception terminating scene launcher") + LauncherRobot.wait_for(self.robot_launchers) + except Exception as e: + LogManager.logger.exception("Exception relaunching robots") def start(self): """ @@ -1049,10 +1088,9 @@ def signal_handler(sign, frame): except Exception as e: LogManager.logger.exception("Exception stopping consumer") - if self.application_process: + if self.application_processes: try: - stop_process_and_children(self.application_process) - self.application_process = None + self.application_processes.kill_all() except Exception as e: LogManager.logger.exception( "Exception stopping application process" @@ -1064,9 +1102,11 @@ def signal_handler(sign, frame): except Exception as e: LogManager.logger.exception("Exception terminating tools launcher") - if self.robot_launcher: + for robot_launcher in self.robot_launchers: + if robot_launcher is None: + continue try: - self.robot_launcher.terminate() + robot_launcher.terminate() except Exception as e: LogManager.logger.exception("Exception terminating robot launcher")