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_ros2_api.py

@@ -1,6 +1,5 @@
 import os
 from typing import List, Any
-import time
 import stat
 
 from .launcher_interface import (
@@ -13,10 +12,6 @@
 
 import logging
 from robotics_application_manager import LogManager
-from gz.transport13 import Node
-
-from gz.msgs10.empty_pb2 import Empty
-from gz.msgs10.scene_pb2 import Scene
 
 
 class LauncherRobotRos2Api(ILauncher):
@@ -26,6 +21,14 @@ class LauncherRobotRos2Api(ILauncher):
     threads: List[Any] = []
 
     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. The
+        manager starts every robot's launch first and then waits for them all to
+        appear in the scene together, so N robots spawn in parallel (~one spawn
+        time, not N) - which also makes every reset faster, since reset respawns
+        the whole group.
+        """
         DRI_PATH = self.get_dri_path()
         ACCELERATION_ENABLED = self.check_device(DRI_PATH)
 
@@ -36,32 +39,17 @@ def run(self, entity, robot_pose, extra_config, callback):
         if extra_config == "None":
             extra_config = ""
 
+        # pass the entity name to the launch. entity is the gazebo model name
+        # (used to spawn/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
-        node = Node()
-        spawned = False
-        while not spawned:
-            a = node.request(
-                f"/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")
-
     def terminate(self):
         LogManager.logger.info(f"Terminating robot launcher")
         for thread in self.threads[:]:

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: