launch every robot and every entrypoint

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,15 @@ 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,
-            )
+        if robot_entities is not None:
+            for robot_entity in robot_entities:
+                node.request(
+                    f"/world/default/remove",
+                    Entity(name=robot_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,6 +38,68 @@ class LauncherRobot(BaseModel):
     entity: str = ""
     start_pose: Optional[list] = []
 
+    @staticmethod
+    def make_names_unique(robot_cfgs):
+        """Rename the robots whose names clash with an earlier one.
+
+        The first robot to use a name keeps it and a later one asking for the
+        same name gets a numbered suffix, so car, vehicle, car becomes car,
+        vehicle, car_1. The entity and the namespace are two different things:
+        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, which is why the namespace is read and written there.
+        """
+        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)
+
+    @staticmethod
+    def wait(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:
+            if robot_launcher is None:
+                continue
+            entities.append(robot_launcher.entity)
+            launchers.extend(robot_launcher.launchers)
+
+        if launchers:
+            launchers[0].wait(entities)
+
     def run(self, entity="", start_pose=None, extra_config=None):
         """Run the robot launcher with an optional start pose."""
         self.entity = entity

robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py

@@ -24,31 +24,34 @@ class LauncherRobotRos2Api(ILauncher):
     module: str
     launch_file: str
     threads: List[Any] = []
+    entity: str = ""
 
     def run(self, entity, robot_pose, extra_config, callback):
         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 = ""
 
         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 +60,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=None):
         for launcher in self.launchers:
-            launcher.reset(robot_entity)
+            launcher.reset(robot_entities)
 
     def pass_msg(self, data):
         for launcher in self.launchers: