launch every robot and every entrypoint

robotics_application_manager/manager/launcher/__init__.py

@@ -1,3 +1,3 @@
 from .launcher_tools import LauncherTools
 from .launcher_scene import LauncherScene
-from .launcher_robot import LauncherRobot
+from .launcher_robot import LauncherRobot, make_names_unique, wait_for_robots

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=[]):
         node = Node()
 
         node.request(
@@ -114,7 +114,7 @@ def reset(self, robot_entity=None):
             10000,
         )
 
-        if robot_entity is not None:
+        for robot_entity in robot_entities:
             node.request(
                 f"/world/default/remove",
                 Entity(name=robot_entity, type=Entity.MODEL),

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
 
@@ -26,6 +27,62 @@
 }
 
 
+def make_names_unique(robot_cfgs):
+    """Rename the robots whose names clash with an earlier one.
+
+    The entity names the model in the simulator and the namespace prefixes the
+    ROS topics, so they are made unique separately. The entity is a field of
+    its own, while the namespace is an argument inside extra_config.
+    """
+    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 wait_for_robots(robot_launchers):
+    """Wait until every robot has spawned in the simulator.
+
+    All the entities are checked in a single loop, so the robots spawn at the
+    same time instead of one after another.
+    """
+    entities = []
+    launchers = []
+    for robot_launcher in robot_launchers:
+        entities.append(robot_launcher.entity)
+        launchers.extend(robot_launcher.launchers)
+
+    if launchers:
+        launchers[0].wait(entities)
+
+
 class LauncherRobot(BaseModel):
     """Class for managing robot launchers in different simulation worlds."""
 

robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py

@@ -37,18 +37,19 @@ def run(self, entity, robot_pose, extra_config, callback):
             extra_config = ""
 
         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):
+        # Wait until every robot entity has spawned
         node = Node()
-        spawned = False
-        while not spawned:
+        pending = set(entities)
+        while pending:
             a = node.request(
                 f"/world/default/scene/info",
                 Empty(),
@@ -57,10 +58,8 @@ def run(self, entity, robot_pose, extra_config, callback):
                 1000,
             )
             if a[0]:
-                for model in a[1].model:
-                    if model.name == entity:
-                        spawned = True
-                        LogManager.logger.info("Robot spawned OK")
+                pending -= {model.name for model in a[1].model}
+        LogManager.logger.info("Robots spawned OK")
 
     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=[]):
         for launcher in self.launchers:
-            launcher.reset(robot_entity)
+            launcher.reset(robot_entities)
 
     def pass_msg(self, data):
         for launcher in self.launchers: