multi-robot support with parallel spawn

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

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",

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")

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")

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: