diff --git a/robotics_application_manager/manager/launcher/__init__.py b/robotics_application_manager/manager/launcher/__init__.py index a31448e..f00d7f4 100644 --- a/robotics_application_manager/manager/launcher/__init__.py +++ b/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 diff --git a/robotics_application_manager/manager/launcher/launcher_gzsim.py b/robotics_application_manager/manager/launcher/launcher_gzsim.py index 3959531..e375100 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=[]): 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), diff --git a/robotics_application_manager/manager/launcher/launcher_robot.py b/robotics_application_manager/manager/launcher/launcher_robot.py index 13f93fd..ec54018 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 @@ -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.""" 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..8ac00d1 100644 --- a/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py +++ b/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") diff --git a/robotics_application_manager/manager/launcher/launcher_tools.py b/robotics_application_manager/manager/launcher/launcher_tools.py index a294de1..3000042 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=[]): 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..312f03d 100644 --- a/robotics_application_manager/manager/manager.py +++ b/robotics_application_manager/manager/manager.py @@ -42,6 +42,8 @@ LauncherScene, LauncherRobot, LauncherTools, + make_names_unique, + wait_for_robots, ) from robotics_application_manager.manager.lint import Lint from robotics_application_manager.manager.editor import serialize_completions @@ -224,10 +226,10 @@ 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 + self.application_processes = [] self.running = True self.linter = Lint() @@ -323,11 +325,8 @@ 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_cfgs = cfg_dict["robot"] + make_names_unique(robot_cfgs) # Launch scene try: @@ -352,8 +351,9 @@ def on_launch_world(self, event): LogManager.logger.info(str(self.scene_launcher)) # Launch robot - self.robot_launcher = None - if robot_cfg["type"] is not None: + self.robot_launchers = [] + self.robot_configs = robot_cfgs + for robot_cfg in robot_cfgs: try: cfg = ConfigurationManager.validate(robot_cfg) LogManager.logger.info("Launching robot from the RB") @@ -361,15 +361,16 @@ def on_launch_world(self, event): 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)) + robot_launcher = LauncherRobot(**cfg.model_dump()) + self.robot_launchers.append(robot_launcher) + LogManager.logger.info(str(robot_launcher)) self.scene_launcher.run() - if self.robot_launcher is not None: - self.robot_launcher.run( + for robot_launcher, robot_cfg in zip(self.robot_launchers, self.robot_configs): + robot_launcher.run( robot_cfg["entity"], robot_cfg["start_pose"], robot_cfg["extra_config"] ) + wait_for_robots(self.robot_launchers) LogManager.logger.info("Launch transition finished") def prepare_custom_world(self, cfg_dict): @@ -740,12 +741,14 @@ 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 + for application_process in self.application_processes: + try: + proc = psutil.Process(application_process.pid) + proc.suspend() + proc.kill() + except Exception: + pass + self.application_processes = [] # Delete old files if os.path.exists("/workspace/code"): @@ -754,12 +757,7 @@ 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"] to_lint = app_cfg["linter"] # Unzip the app @@ -771,13 +769,18 @@ def on_run_application(self, event): zip_ref.extractall("/workspace/code") zip_ref.close() - if not os.path.isfile(entrypoint): - LogManager.logger.info("User code not found") - raise Exception("User code not found") + for entrypoint in entrypoints: + if not os.path.isfile(entrypoint): + LogManager.logger.info("User code not found") + raise Exception("User code not found") - _, file_extension = os.path.splitext(entrypoint) + needs_compile = False + for entrypoint in entrypoints: + _, file_extension = os.path.splitext(entrypoint) + if file_extension == ".cpp" or entrypoint.endswith(".launch.py"): + needs_compile = True - if file_extension == ".cpp" or entrypoint.endswith(".launch.py"): + if needs_compile: fds = os.listdir("/dev/pts/") console_fd = str(max(map(int, fds[:-1]))) @@ -797,62 +800,71 @@ 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( - [ - f"source /workspace/code/install/setup.bash && ros2 launch {entrypoint}" - ], - stdin=open("/dev/pts/" + console_fd, "r"), - stdout=open("/dev/pts/" + console_fd, "w"), - stderr=sys.stdout, - bufsize=1024, - universal_newlines=True, - shell=True, - executable="/bin/bash", - start_new_session=True, - ) - else: + for entrypoint in entrypoints: + _, file_extension = os.path.splitext(entrypoint) + + if file_extension == ".cpp" or entrypoint.endswith(".launch.py"): + fds = os.listdir("/dev/pts/") + console_fd = str(max(map(int, fds[:-1]))) + + self.unpause_sim() + if entrypoint.endswith(".launch.py"): + application_process = subprocess.Popen( + [ + f"source /workspace/code/install/setup.bash && ros2 launch {entrypoint}" + ], + stdin=open("/dev/pts/" + console_fd, "r"), + stdout=open("/dev/pts/" + console_fd, "w"), + stderr=sys.stdout, + bufsize=1024, + universal_newlines=True, + shell=True, + executable="/bin/bash", + start_new_session=True, + ) + else: - self.application_process = subprocess.Popen( - [ - "source /workspace/code/install/setup.bash && ros2 run academy academyCode" - ], - stdin=open("/dev/pts/" + console_fd, "r"), - stdout=open("/dev/pts/" + console_fd, "w"), - stderr=sys.stdout, - bufsize=1024, - universal_newlines=True, - shell=True, - executable="/bin/bash", - start_new_session=True, - ) - return + application_process = subprocess.Popen( + [ + "source /workspace/code/install/setup.bash && ros2 run academy academyCode" + ], + stdin=open("/dev/pts/" + console_fd, "r"), + stdout=open("/dev/pts/" + console_fd, "w"), + stderr=sys.stdout, + bufsize=1024, + universal_newlines=True, + shell=True, + executable="/bin/bash", + start_new_session=True, + ) + self.application_processes.append(application_process) + continue - # Pass the linter - errors = self.linter.evaluate_source_code(to_lint) - failed_linter = False - - for error in errors: - if error != "": - failed_linter = True - self.write_to_tool_terminal(error + "\n\n") - - if failed_linter: - raise Exception(errors) - - 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, - ) + # Pass the linter + errors = self.linter.evaluate_source_code(to_lint) + failed_linter = False + + for error in errors: + if error != "": + failed_linter = True + self.write_to_tool_terminal(error + "\n\n") + + if failed_linter: + raise Exception(errors) + + fds = os.listdir("/dev/pts/") + console_fd = str(max(map(int, fds[:-1]))) + + self.unpause_sim() + 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, + ) + self.application_processes.append(application_process) LogManager.logger.info("Run application transition finished") @@ -866,10 +878,11 @@ 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 + for application_process in self.application_processes: + stop_process_and_children(application_process) + self.application_processes = [] self.pause_sim() self.reset_sim() except Exception: @@ -895,9 +908,10 @@ 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: + robot_launcher.terminate() + self.robot_launchers = [] + self.robot_configs = [] def on_disconnect(self, event): """ @@ -907,10 +921,11 @@ 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 + for application_process in self.application_processes: + stop_process_and_children(application_process) + self.application_processes = [] except Exception as e: LogManager.logger.exception("Exception stopping application process") @@ -920,9 +935,9 @@ 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: try: - self.robot_launcher.terminate() + robot_launcher.terminate() except Exception as e: LogManager.logger.exception("Exception terminating robot launcher") @@ -943,15 +958,16 @@ 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: + for application_process in self.application_processes: + proc = psutil.Process(application_process.pid) + children = proc.children(recursive=True) + children.append(proc) + for p in children: + try: + p.suspend() + except psutil.NoSuchProcess: + pass self.pause_sim() else: LogManager.logger.warning( @@ -967,15 +983,16 @@ 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: + for application_process in self.application_processes: + proc = psutil.Process(application_process.pid) + children = proc.children(recursive=True) + children.append(proc) + for p in children: + try: + p.resume() + except psutil.NoSuchProcess: + pass self.unpause_sim() else: LogManager.logger.warning( @@ -1005,27 +1022,26 @@ 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() + for robot_launcher in self.robot_launchers: + robot_launcher.terminate() try: - entity = None - if self.robot_config is not None: - entity = self.robot_config["entity"] - self.tools_launcher.reset(entity) + entities = [robot_cfg["entity"] for robot_cfg in self.robot_configs] + 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: + for robot_launcher, robot_cfg in zip(self.robot_launchers, self.robot_configs): try: - self.robot_launcher.run( - self.robot_config["entity"], - self.robot_config["start_pose"], - self.robot_config["extra_config"], + robot_launcher.run( + robot_cfg["entity"], + robot_cfg["start_pose"], + robot_cfg["extra_config"], ) except Exception as e: LogManager.logger.exception("Exception terminating scene launcher") + wait_for_robots(self.robot_launchers) def start(self): """ @@ -1049,10 +1065,11 @@ 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 + for application_process in self.application_processes: + stop_process_and_children(application_process) + self.application_processes = [] except Exception as e: LogManager.logger.exception( "Exception stopping application process" @@ -1064,9 +1081,9 @@ 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: try: - self.robot_launcher.terminate() + robot_launcher.terminate() except Exception as e: LogManager.logger.exception("Exception terminating robot launcher")