From e23ee91ac11cd5981f09633b9e526186d9ff0d73 Mon Sep 17 00:00:00 2001 From: anish Date: Tue, 28 Jul 2026 14:31:07 +0530 Subject: [PATCH 1/6] launch every robot and every entrypoint --- .../manager/launcher/launcher_gzsim.py | 19 +- .../manager/launcher/launcher_robot.py | 63 +++++ .../launcher/launcher_robot_ros2_api.py | 19 +- .../manager/launcher/launcher_tools.py | 4 +- .../manager/manager.py | 259 ++++++++++-------- 5 files changed, 234 insertions(+), 130 deletions(-) diff --git a/robotics_application_manager/manager/launcher/launcher_gzsim.py b/robotics_application_manager/manager/launcher/launcher_gzsim.py index 3959531..d14a517 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=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", diff --git a/robotics_application_manager/manager/launcher/launcher_robot.py b/robotics_application_manager/manager/launcher/launcher_robot.py index 13f93fd..db86cd0 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 @@ -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 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..673b926 100644 --- a/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py +++ b/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py @@ -24,6 +24,7 @@ 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() @@ -31,24 +32,26 @@ def run(self, entity, robot_pose, extra_config, callback): 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") diff --git a/robotics_application_manager/manager/launcher/launcher_tools.py b/robotics_application_manager/manager/launcher/launcher_tools.py index a294de1..32f6778 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=None): 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..8ccfa51 100644 --- a/robotics_application_manager/manager/manager.py +++ b/robotics_application_manager/manager/manager.py @@ -224,10 +224,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 +323,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"] + LauncherRobot.make_names_unique(robot_cfgs) # Launch scene try: @@ -352,24 +349,33 @@ 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: + if robot_cfg["type"] is None: + self.robot_launchers.append(None) + continue + try: cfg = ConfigurationManager.validate(robot_cfg) LogManager.logger.info("Launching robot from the RB") LogManager.logger.info(cfg) except ValueError as e: LogManager.logger.error(f"Configuration validation failed: {e}") + self.robot_launchers.append(None) + continue - self.robot_launcher = LauncherRobot(**cfg.model_dump()) - self.robot_config = robot_cfg - LogManager.logger.info(str(self.robot_launcher)) + self.robot_launchers.append(LauncherRobot(**cfg.model_dump())) + LogManager.logger.info(str(self.robot_launchers[-1])) self.scene_launcher.run() - if self.robot_launcher is not None: - self.robot_launcher.run( + for launcher, robot_cfg in zip(self.robot_launchers, self.robot_configs): + if launcher is None: + continue + launcher.run( robot_cfg["entity"], robot_cfg["start_pose"], robot_cfg["extra_config"] ) + LauncherRobot.wait(self.robot_launchers) LogManager.logger.info("Launch transition finished") def prepare_custom_world(self, cfg_dict): @@ -740,12 +746,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 +762,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,16 +774,22 @@ 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) + # The workspace is built once, as soon as any entrypoint needs it + needs_build = any( + os.path.splitext(entrypoint)[1] == ".cpp" + or entrypoint.endswith(".launch.py") + for entrypoint in entrypoints + ) - if file_extension == ".cpp" or entrypoint.endswith(".launch.py"): - fds = os.listdir("/dev/pts/") - console_fd = str(max(map(int, fds[:-1]))) + fds = os.listdir("/dev/pts/") + console_fd = str(max(map(int, fds[:-1]))) + if needs_build: compile_process = subprocess.Popen( [ "cd /workspace/code && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && cd ../.." @@ -797,9 +806,36 @@ def on_run_application(self, event): if returncode != 0: raise Exception("Failed to compile") - self.unpause_sim() + # Pass the linter. Only the Python entrypoints are linted + if any( + os.path.splitext(entrypoint)[1] != ".cpp" + and not entrypoint.endswith(".launch.py") + for entrypoint in entrypoints + ): + 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) + + # The libraries are extracted to the root of the code, while an + # entrypoint may live in a subdirectory of it. Python only looks next to + # the file it runs, so the root is added to the path to keep the + # libraries importable from anywhere in the code + environment = os.environ.copy() + environment["PYTHONPATH"] = "/workspace/code:" + environment.get( + "PYTHONPATH", "" + ) + + self.unpause_sim() + for entrypoint in entrypoints: if entrypoint.endswith(".launch.py"): - self.application_process = subprocess.Popen( + application_process = subprocess.Popen( [ f"source /workspace/code/install/setup.bash && ros2 launch {entrypoint}" ], @@ -811,10 +847,10 @@ def on_run_application(self, event): shell=True, executable="/bin/bash", start_new_session=True, + env=environment, ) - else: - - self.application_process = subprocess.Popen( + elif os.path.splitext(entrypoint)[1] == ".cpp": + application_process = subprocess.Popen( [ "source /workspace/code/install/setup.bash && ros2 run academy academyCode" ], @@ -826,33 +862,19 @@ def on_run_application(self, event): shell=True, executable="/bin/bash", start_new_session=True, + env=environment, ) - return - - # 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, - ) + else: + 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, + env=environment, + ) + self.application_processes.append(application_process) LogManager.logger.info("Run application transition finished") @@ -866,10 +888,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 +918,11 @@ 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: + if robot_launcher is not None: + robot_launcher.terminate() + self.robot_launchers = [] + self.robot_configs = [] def on_disconnect(self, event): """ @@ -907,10 +932,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 +946,11 @@ 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: + if robot_launcher is None: + continue try: - self.robot_launcher.terminate() + robot_launcher.terminate() except Exception as e: LogManager.logger.exception("Exception terminating robot launcher") @@ -943,15 +971,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 +996,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 +1035,33 @@ 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: + if robot_launcher is not None: + 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 launcher, robot_cfg in zip(self.robot_launchers, self.robot_configs) + if launcher is not None + ] + 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 launcher, robot_cfg in zip(self.robot_launchers, self.robot_configs): + if launcher is None: + continue try: - self.robot_launcher.run( - self.robot_config["entity"], - self.robot_config["start_pose"], - self.robot_config["extra_config"], + launcher.run( + robot_cfg["entity"], + robot_cfg["start_pose"], + robot_cfg["extra_config"], ) except Exception as e: LogManager.logger.exception("Exception terminating scene launcher") + LauncherRobot.wait(self.robot_launchers) def start(self): """ @@ -1049,10 +1085,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 +1101,11 @@ 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: + if robot_launcher is None: + continue try: - self.robot_launcher.terminate() + robot_launcher.terminate() except Exception as e: LogManager.logger.exception("Exception terminating robot launcher") From 94c20145f871b455ae2e3d51332728f06f1302dc Mon Sep 17 00:00:00 2001 From: anish Date: Tue, 28 Jul 2026 16:29:57 +0530 Subject: [PATCH 2/6] addressing first 6 comments --- .../manager/launcher/launcher_robot.py | 118 +++++++++--------- .../launcher/launcher_robot_ros2_api.py | 2 - .../manager/launcher/launcher_tools.py | 2 +- 3 files changed, 57 insertions(+), 65 deletions(-) diff --git a/robotics_application_manager/manager/launcher/launcher_robot.py b/robotics_application_manager/manager/launcher/launcher_robot.py index db86cd0..ec54018 100644 --- a/robotics_application_manager/manager/launcher/launcher_robot.py +++ b/robotics_application_manager/manager/launcher/launcher_robot.py @@ -27,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.""" @@ -38,68 +94,6 @@ 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 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 673b926..8ac00d1 100644 --- a/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py +++ b/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py @@ -24,7 +24,6 @@ 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() @@ -32,7 +31,6 @@ def run(self, entity, robot_pose, extra_config, callback): logging.getLogger("roslaunch").setLevel(logging.CRITICAL) - self.entity = entity x, y, z, R, P, Y = robot_pose if extra_config == "None": diff --git a/robotics_application_manager/manager/launcher/launcher_tools.py b/robotics_application_manager/manager/launcher/launcher_tools.py index 32f6778..3000042 100644 --- a/robotics_application_manager/manager/launcher/launcher_tools.py +++ b/robotics_application_manager/manager/launcher/launcher_tools.py @@ -121,7 +121,7 @@ def unpause(self): for launcher in self.launchers: launcher.unpause() - def reset(self, robot_entities=None): + def reset(self, robot_entities=[]): for launcher in self.launchers: launcher.reset(robot_entities) From 92bca1140521580de69e5cd86a0bfbad61a4714d Mon Sep 17 00:00:00 2001 From: anish Date: Tue, 28 Jul 2026 17:30:03 +0530 Subject: [PATCH 3/6] Address remaining comments --- .../manager/launcher/__init__.py | 2 +- .../manager/launcher/launcher_gzsim.py | 19 +- .../manager/manager.py | 193 ++++++++---------- 3 files changed, 90 insertions(+), 124 deletions(-) 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 d14a517..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_entities=None): + def reset(self, robot_entities=[]): node = Node() node.request( @@ -114,15 +114,14 @@ def reset(self, robot_entities=None): 10000, ) - 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, - ) + 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", diff --git a/robotics_application_manager/manager/manager.py b/robotics_application_manager/manager/manager.py index 8ccfa51..2a35ebd 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 @@ -324,7 +326,7 @@ def on_launch_world(self, event): cfg_dict = event.kwargs.get("data", {}) scene_cfg = cfg_dict["scene"] robot_cfgs = cfg_dict["robot"] - LauncherRobot.make_names_unique(robot_cfgs) + make_names_unique(robot_cfgs) # Launch scene try: @@ -352,30 +354,23 @@ def on_launch_world(self, event): self.robot_launchers = [] self.robot_configs = robot_cfgs for robot_cfg in robot_cfgs: - if robot_cfg["type"] is None: - self.robot_launchers.append(None) - continue - try: cfg = ConfigurationManager.validate(robot_cfg) LogManager.logger.info("Launching robot from the RB") LogManager.logger.info(cfg) except ValueError as e: LogManager.logger.error(f"Configuration validation failed: {e}") - self.robot_launchers.append(None) - continue - self.robot_launchers.append(LauncherRobot(**cfg.model_dump())) - LogManager.logger.info(str(self.robot_launchers[-1])) + robot_launcher = LauncherRobot(**cfg.model_dump()) + self.robot_launchers.append(robot_launcher) + LogManager.logger.info(str(robot_launcher)) self.scene_launcher.run() - for launcher, robot_cfg in zip(self.robot_launchers, self.robot_configs): - if launcher is None: - continue - 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"] ) - LauncherRobot.wait(self.robot_launchers) + wait_for_robots(self.robot_launchers) LogManager.logger.info("Launch transition finished") def prepare_custom_world(self, cfg_dict): @@ -779,39 +774,62 @@ def on_run_application(self, event): LogManager.logger.info("User code not found") raise Exception("User code not found") - # The workspace is built once, as soon as any entrypoint needs it - needs_build = any( - os.path.splitext(entrypoint)[1] == ".cpp" - or entrypoint.endswith(".launch.py") - for entrypoint in entrypoints - ) + _, file_extension = os.path.splitext(entrypoint) - fds = os.listdir("/dev/pts/") - console_fd = str(max(map(int, fds[:-1]))) + if file_extension == ".cpp" or entrypoint.endswith(".launch.py"): + fds = os.listdir("/dev/pts/") + console_fd = str(max(map(int, fds[:-1]))) - if needs_build: - compile_process = subprocess.Popen( - [ - "cd /workspace/code && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && cd ../.." - ], - stdin=open("/dev/pts/" + console_fd, "r"), - stdout=open("/dev/pts/" + console_fd, "w"), - stderr=open("/dev/pts/" + console_fd, "w"), - bufsize=1024, - universal_newlines=True, - shell=True, - executable="/bin/bash", - ) - returncode = compile_process.wait() - if returncode != 0: - raise Exception("Failed to compile") - - # Pass the linter. Only the Python entrypoints are linted - if any( - os.path.splitext(entrypoint)[1] != ".cpp" - and not entrypoint.endswith(".launch.py") - for entrypoint in entrypoints - ): + compile_process = subprocess.Popen( + [ + "cd /workspace/code && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && cd ../.." + ], + stdin=open("/dev/pts/" + console_fd, "r"), + stdout=open("/dev/pts/" + console_fd, "w"), + stderr=open("/dev/pts/" + console_fd, "w"), + bufsize=1024, + universal_newlines=True, + shell=True, + executable="/bin/bash", + ) + returncode = compile_process.wait() + if returncode != 0: + raise Exception("Failed to compile") + + 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: + + 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 @@ -823,57 +841,18 @@ def on_run_application(self, event): if failed_linter: raise Exception(errors) - # The libraries are extracted to the root of the code, while an - # entrypoint may live in a subdirectory of it. Python only looks next to - # the file it runs, so the root is added to the path to keep the - # libraries importable from anywhere in the code - environment = os.environ.copy() - environment["PYTHONPATH"] = "/workspace/code:" + environment.get( - "PYTHONPATH", "" - ) + fds = os.listdir("/dev/pts/") + console_fd = str(max(map(int, fds[:-1]))) - self.unpause_sim() - for entrypoint in entrypoints: - 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, - env=environment, - ) - elif os.path.splitext(entrypoint)[1] == ".cpp": - 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, - env=environment, - ) - else: - 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, - env=environment, - ) + 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") @@ -919,8 +898,7 @@ def on_terminate_world(self, event): self.scene_launcher = None self.world_type = None for robot_launcher in self.robot_launchers: - if robot_launcher is not None: - robot_launcher.terminate() + robot_launcher.terminate() self.robot_launchers = [] self.robot_configs = [] @@ -947,8 +925,6 @@ def on_disconnect(self, event): LogManager.logger.exception("Exception terminating tools launcher") for robot_launcher in self.robot_launchers: - if robot_launcher is None: - continue try: robot_launcher.terminate() except Exception as e: @@ -1036,32 +1012,25 @@ def reset_sim(self): and relaunches the robot if a launcher is available. """ for robot_launcher in self.robot_launchers: - if robot_launcher is not None: - robot_launcher.terminate() + robot_launcher.terminate() try: - entities = [ - robot_cfg["entity"] - for launcher, robot_cfg in zip(self.robot_launchers, self.robot_configs) - if launcher is not None - ] + 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") - for launcher, robot_cfg in zip(self.robot_launchers, self.robot_configs): - if launcher is None: - continue + for robot_launcher, robot_cfg in zip(self.robot_launchers, self.robot_configs): try: - launcher.run( + 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") - LauncherRobot.wait(self.robot_launchers) + wait_for_robots(self.robot_launchers) def start(self): """ @@ -1102,8 +1071,6 @@ def signal_handler(sign, frame): LogManager.logger.exception("Exception terminating tools launcher") for robot_launcher in self.robot_launchers: - if robot_launcher is None: - continue try: robot_launcher.terminate() except Exception as e: From df1d26dc6f0b93d413f673ba5448cc131d6d2ea5 Mon Sep 17 00:00:00 2001 From: anish Date: Wed, 29 Jul 2026 18:07:47 +0530 Subject: [PATCH 4/6] changes to entrypoint modifying only run entrypoint --- .../manager/manager.py | 72 ++++++++++--------- 1 file changed, 37 insertions(+), 35 deletions(-) diff --git a/robotics_application_manager/manager/manager.py b/robotics_application_manager/manager/manager.py index 2a35ebd..94a5348 100644 --- a/robotics_application_manager/manager/manager.py +++ b/robotics_application_manager/manager/manager.py @@ -774,29 +774,30 @@ def on_run_application(self, event): LogManager.logger.info("User code not found") raise Exception("User code not found") - _, 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]))) - - compile_process = subprocess.Popen( - [ - "cd /workspace/code && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && cd ../.." - ], - stdin=open("/dev/pts/" + console_fd, "r"), - stdout=open("/dev/pts/" + console_fd, "w"), - stderr=open("/dev/pts/" + console_fd, "w"), - bufsize=1024, - universal_newlines=True, - shell=True, - executable="/bin/bash", - ) - returncode = compile_process.wait() - if returncode != 0: - raise Exception("Failed to compile") + _, file_extension = os.path.splitext(entrypoints[0]) + + if file_extension == ".cpp" or entrypoints[0].endswith(".launch.py"): + fds = os.listdir("/dev/pts/") + console_fd = str(max(map(int, fds[:-1]))) + + compile_process = subprocess.Popen( + [ + "cd /workspace/code && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && cd ../.." + ], + stdin=open("/dev/pts/" + console_fd, "r"), + stdout=open("/dev/pts/" + console_fd, "w"), + stderr=open("/dev/pts/" + console_fd, "w"), + bufsize=1024, + universal_newlines=True, + shell=True, + executable="/bin/bash", + ) + returncode = compile_process.wait() + if returncode != 0: + raise Exception("Failed to compile") - self.unpause_sim() + self.unpause_sim() + for entrypoint in entrypoints: if entrypoint.endswith(".launch.py"): application_process = subprocess.Popen( [ @@ -827,24 +828,25 @@ def on_run_application(self, event): start_new_session=True, ) self.application_processes.append(application_process) - continue + return - # Pass the linter - errors = self.linter.evaluate_source_code(to_lint) - failed_linter = False + # 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") + for error in errors: + if error != "": + failed_linter = True + self.write_to_tool_terminal(error + "\n\n") - if failed_linter: - raise Exception(errors) + if failed_linter: + raise Exception(errors) - fds = os.listdir("/dev/pts/") - console_fd = str(max(map(int, fds[:-1]))) + fds = os.listdir("/dev/pts/") + console_fd = str(max(map(int, fds[:-1]))) - self.unpause_sim() + self.unpause_sim() + for entrypoint in entrypoints: application_process = subprocess.Popen( ["python3", entrypoint], stdin=open("/dev/pts/" + console_fd, "r"), From c4ea74776609e62af59f3e396fd981977f29cccc Mon Sep 17 00:00:00 2001 From: anish Date: Wed, 29 Jul 2026 23:58:49 +0530 Subject: [PATCH 5/6] final one on entrypoints --- .../manager/manager.py | 45 +++++++++++-------- 1 file changed, 27 insertions(+), 18 deletions(-) diff --git a/robotics_application_manager/manager/manager.py b/robotics_application_manager/manager/manager.py index 94a5348..312f03d 100644 --- a/robotics_application_manager/manager/manager.py +++ b/robotics_application_manager/manager/manager.py @@ -774,9 +774,13 @@ def on_run_application(self, event): LogManager.logger.info("User code not found") raise Exception("User code not found") - _, file_extension = os.path.splitext(entrypoints[0]) + 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 entrypoints[0].endswith(".launch.py"): + if needs_compile: fds = os.listdir("/dev/pts/") console_fd = str(max(map(int, fds[:-1]))) @@ -796,8 +800,14 @@ def on_run_application(self, event): if returncode != 0: raise Exception("Failed to compile") - self.unpause_sim() - for entrypoint in entrypoints: + 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( [ @@ -828,25 +838,24 @@ def on_run_application(self, event): start_new_session=True, ) self.application_processes.append(application_process) - return + continue - # Pass the linter - errors = self.linter.evaluate_source_code(to_lint) - failed_linter = False + # 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") + for error in errors: + if error != "": + failed_linter = True + self.write_to_tool_terminal(error + "\n\n") - if failed_linter: - raise Exception(errors) + if failed_linter: + raise Exception(errors) - fds = os.listdir("/dev/pts/") - console_fd = str(max(map(int, fds[:-1]))) + fds = os.listdir("/dev/pts/") + console_fd = str(max(map(int, fds[:-1]))) - self.unpause_sim() - for entrypoint in entrypoints: + self.unpause_sim() application_process = subprocess.Popen( ["python3", entrypoint], stdin=open("/dev/pts/" + console_fd, "r"), From 58a430703705346dc3e3ac5fa990ad125c1d3864 Mon Sep 17 00:00:00 2001 From: anish Date: Thu, 30 Jul 2026 16:12:29 +0530 Subject: [PATCH 6/6] linter --- .../manager/manager.py | 40 ++++++++----------- 1 file changed, 17 insertions(+), 23 deletions(-) diff --git a/robotics_application_manager/manager/manager.py b/robotics_application_manager/manager/manager.py index 312f03d..4df5aad 100644 --- a/robotics_application_manager/manager/manager.py +++ b/robotics_application_manager/manager/manager.py @@ -774,6 +774,9 @@ def on_run_application(self, event): LogManager.logger.info("User code not found") raise Exception("User code not found") + fds = os.listdir("/dev/pts/") + console_fd = str(max(map(int, fds[:-1]))) + needs_compile = False for entrypoint in entrypoints: _, file_extension = os.path.splitext(entrypoint) @@ -781,9 +784,6 @@ def on_run_application(self, event): needs_compile = True if needs_compile: - fds = os.listdir("/dev/pts/") - console_fd = str(max(map(int, fds[:-1]))) - compile_process = subprocess.Popen( [ "cd /workspace/code && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && cd ../.." @@ -800,14 +800,24 @@ def on_run_application(self, event): if returncode != 0: raise Exception("Failed to compile") + # 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) + + self.unpause_sim() + 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( [ @@ -840,22 +850,6 @@ def on_run_application(self, event): 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() application_process = subprocess.Popen( ["python3", entrypoint], stdin=open("/dev/pts/" + console_fd, "r"),