diff --git a/robotics_application_manager/manager/launcher/launcher_console.py b/robotics_application_manager/manager/launcher/launcher_console.py index 4a678ca..6848118 100644 --- a/robotics_application_manager/manager/launcher/launcher_console.py +++ b/robotics_application_manager/manager/launcher/launcher_console.py @@ -46,7 +46,7 @@ def pause(self): def unpause(self): pass - def reset(self, robot_entity=None): + def reset(self, robot_entities=[]): pass def is_running(self): 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_o3de.py b/robotics_application_manager/manager/launcher/launcher_o3de.py index 289c148..42a0cdc 100644 --- a/robotics_application_manager/manager/launcher/launcher_o3de.py +++ b/robotics_application_manager/manager/launcher/launcher_o3de.py @@ -50,7 +50,7 @@ def unpause(self): self.running = True pass - def reset(self, robot_entity=None): + def reset(self, robot_entities=[]): # TODO: add reset pass diff --git a/robotics_application_manager/manager/launcher/launcher_o3de_api.py b/robotics_application_manager/manager/launcher/launcher_o3de_api.py index 5824c6b..e6f2ade 100644 --- a/robotics_application_manager/manager/launcher/launcher_o3de_api.py +++ b/robotics_application_manager/manager/launcher/launcher_o3de_api.py @@ -90,3 +90,6 @@ def terminate(self): bufsize=1024, universal_newlines=True, ) + + def wait_robot_spawn(self, entities): + pass 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..ed642b9 100644 --- a/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py +++ b/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py @@ -13,10 +13,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): @@ -37,31 +33,14 @@ 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 - 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[:]: diff --git a/robotics_application_manager/manager/launcher/launcher_ros2_api.py b/robotics_application_manager/manager/launcher/launcher_ros2_api.py index d58fa41..409504c 100644 --- a/robotics_application_manager/manager/launcher/launcher_ros2_api.py +++ b/robotics_application_manager/manager/launcher/launcher_ros2_api.py @@ -48,3 +48,6 @@ def terminate(self): thread.terminate() thread.join() self.threads.remove(thread) + + def wait_robot_spawn(self, entities): + pass diff --git a/robotics_application_manager/manager/launcher/launcher_ros2_gz_api.py b/robotics_application_manager/manager/launcher/launcher_ros2_gz_api.py new file mode 100644 index 0000000..e941443 --- /dev/null +++ b/robotics_application_manager/manager/launcher/launcher_ros2_gz_api.py @@ -0,0 +1,70 @@ +import os +import sys +from typing import List, Any +import time +import stat + +from .launcher_interface import ( + ILauncher, + LauncherException, +) +from robotics_application_manager.manager.docker_thread import DockerThread +import subprocess +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 +import logging + + +class LauncherRos2GzApi(ILauncher): + type: str + module: str + launch_file: str + threads: List[Any] = [] + + def run(self, callback): + DRI_PATH = self.get_dri_path() + ACCELERATION_ENABLED = self.check_device(DRI_PATH) + + logging.getLogger("roslaunch").setLevel(logging.CRITICAL) + + xserver_cmd = f"/usr/bin/Xorg -quiet -noreset +extension GLX +extension RANDR +extension RENDER -logfile ./xdummy.log -config ./xorg.conf :0" + xserver_thread = DockerThread(xserver_cmd) + xserver_thread.start() + self.threads.append(xserver_thread) + + if ACCELERATION_ENABLED: + exercise_launch_cmd = f"source /.env;export VGL_DISPLAY={DRI_PATH}; vglrun ros2 launch {self.launch_file}" + else: + exercise_launch_cmd = f"source /.env;ros2 launch {self.launch_file}" + + exercise_launch_thread = DockerThread(exercise_launch_cmd) + exercise_launch_thread.start() + self.threads.append(exercise_launch_thread) + + def terminate(self): + LogManager.logger.info(f"Terminating world launcher") + for thread in self.threads[:]: + if thread.is_alive(): + thread.terminate() + thread.join() + self.threads.remove(thread) + + def wait_robots_spawn(self, entities): + # Wait until robots entities has spawned + node = Node() + missing_entities = entities + while len(missing_entities) > 0: + resp, output = node.request( + f"/world/default/scene/info", + Empty(), + Empty, + Scene, + 1000, + ) + if resp: + for model in output.model: + if model.name in missing_entities: + missing_entities.remove(model.name) + LogManager.logger.info(f"Robot ${model.name} spawned OK") diff --git a/robotics_application_manager/manager/launcher/launcher_rviz.py b/robotics_application_manager/manager/launcher/launcher_rviz.py index d2eb606..d08c05e 100644 --- a/robotics_application_manager/manager/launcher/launcher_rviz.py +++ b/robotics_application_manager/manager/launcher/launcher_rviz.py @@ -66,7 +66,7 @@ def pause(self): def unpause(self): pass - def reset(self, robot_entity=None): + def reset(self, robot_entities=[]): pass def is_running(self): diff --git a/robotics_application_manager/manager/launcher/launcher_scene.py b/robotics_application_manager/manager/launcher/launcher_scene.py index afe2b9c..61cd650 100644 --- a/robotics_application_manager/manager/launcher/launcher_scene.py +++ b/robotics_application_manager/manager/launcher/launcher_scene.py @@ -14,7 +14,7 @@ "2": [ { "type": "gz", - "module": "ros2_api", + "module": "ros2_gz_api", "parameters": [], "launch_file": [], } @@ -86,6 +86,10 @@ def process_terminated(name, exit_code): def launch_command(self, configuration): pass + def wait_robots_spawn(self, entities): + for launcher in self.launchers: + launcher.wait_robots_spawn(entities) + class LauncherSceneException(Exception): def __init__(self, message): diff --git a/robotics_application_manager/manager/launcher/launcher_state_monitor.py b/robotics_application_manager/manager/launcher/launcher_state_monitor.py index 7b4767d..94208d6 100644 --- a/robotics_application_manager/manager/launcher/launcher_state_monitor.py +++ b/robotics_application_manager/manager/launcher/launcher_state_monitor.py @@ -40,7 +40,7 @@ def pause(self): def unpause(self): pass - def reset(self, robot_entity=None): + def reset(self, robot_entities=[]): pass def died(self): 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/launcher/launcher_web_gui.py b/robotics_application_manager/manager/launcher/launcher_web_gui.py index a93e222..00f9306 100644 --- a/robotics_application_manager/manager/launcher/launcher_web_gui.py +++ b/robotics_application_manager/manager/launcher/launcher_web_gui.py @@ -40,7 +40,7 @@ def pause(self): def unpause(self): pass - def reset(self, robot_entity=None): + def reset(self, robot_entities=[]): pass def died(self): diff --git a/robotics_application_manager/manager/manager.py b/robotics_application_manager/manager/manager.py index 770e4e3..9b5a44c 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,40 @@ def on_launch_world(self, event): """ cfg_dict = event.kwargs.get("data", {}) scene_cfg = cfg_dict["scene"] - robot_cfg = cfg_dict["robot"] + robot_cfgs = cfg_dict["robot"] - # Backwards compatibility for now - if isinstance(robot_cfg, list): - robot_cfg = robot_cfg[0] + def make_unique(cfgs, key, param=None): + new_values = set() + + for cfg in cfgs: + new_val = cfg[key] + if new_val is None: + continue + + if param is not None: + new_val = re.search(rf"{param}:=(\S+)", new_val) + if new_val is None: + continue + new_val = new_val.group(1) + + index = 1 + while new_val in new_values: + new_val = f"{new_val}_{index}" + index += 1 + + if param is not None: + cfg[key] = re.sub( + rf"{param}:=\S+", + f"{param}:={new_val}", + cfg[key], + ) + else: + cfg[key] = new_val + + new_values.add(new_val) + + make_unique(robot_cfgs, "entity") + make_unique(robot_cfgs, "extra_config", param="namespace") # Launch scene try: @@ -352,8 +381,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 = [] + for robot_cfg in robot_cfgs: try: cfg = ConfigurationManager.validate(robot_cfg) LogManager.logger.info("Launching robot from the RB") @@ -361,15 +391,21 @@ 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) + self.robot_configs.append(robot_cfg) + LogManager.logger.info(str(robot_launcher)) self.scene_launcher.run() - if self.robot_launcher is not None: - self.robot_launcher.run( - robot_cfg["entity"], robot_cfg["start_pose"], robot_cfg["extra_config"] - ) + + robots_data = zip(self.robot_launchers, self.robot_configs) + entities = [] + for launcher, cfg in robots_data: + launcher.run(cfg["entity"], cfg["start_pose"], cfg["extra_config"]) + entities.append(cfg["entity"]) + + self.scene_launcher.wait_robots_spawn(entities) + LogManager.logger.info("Launch transition finished") def prepare_custom_world(self, cfg_dict): @@ -740,12 +776,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 process in self.application_processes: + try: + proc = psutil.Process(process.pid) + proc.suspend() + proc.kill() + except Exception: + pass + self.application_processes = [] # Delete old files if os.path.exists("/workspace/code"): @@ -754,12 +792,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 +804,29 @@ 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") + fds = os.listdir("/dev/pts/") + console_fd = str(max(map(int, fds[:-1]))) + + # 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) - _, file_extension = os.path.splitext(entrypoint) + needs_compile = False - if file_extension == ".cpp" or entrypoint.endswith(".launch.py"): - fds = os.listdir("/dev/pts/") - console_fd = str(max(map(int, fds[:-1]))) + for entrypoint in entrypoints: + if entrypoint.endswith((".launch.py", ".cpp")): + needs_compile = True + break + if needs_compile: compile_process = subprocess.Popen( [ "cd /workspace/code && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && cd ../.." @@ -797,62 +843,46 @@ 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: + if not os.path.isfile(entrypoint): + LogManager.logger.info("User code not found") + raise Exception("User code not found") - 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 - - # 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") + command = ["python3", entrypoint] + executable = None + shell = False - if failed_linter: - raise Exception(errors) + if entrypoint.endswith(".launch.py"): + command = [ + f"source /workspace/code/install/setup.bash && ros2 launch {entrypoint}" + ] + executable = "/bin/bash" + shell = True + elif entrypoint.endswith(".cpp"): + command = [ + "source /workspace/code/install/setup.bash && ros2 run academy academyCode" + ] + executable = "/bin/bash" + shell = True + + proc = subprocess.Popen( + command, + stdin=open("/dev/pts/" + console_fd, "r"), + stdout=open("/dev/pts/" + console_fd, "w"), + stderr=sys.stdout, + bufsize=1024, + universal_newlines=True, + shell=shell, + executable=executable, + start_new_session=True, + ) - fds = os.listdir("/dev/pts/") - console_fd = str(max(map(int, fds[:-1]))) + proc.send_signal(signal.SIGSTOP) + self.application_processes.append(proc) 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, - ) + for app in self.application_processes: + app.send_signal(signal.SIGCONT) LogManager.logger.info("Run application transition finished") @@ -866,16 +896,18 @@ def on_terminate_application(self, event): Parameters: event: The event object associated with the termination request. """ - if self.application_process: + for process in self.application_processes: try: - stop_process_and_children(self.application_process) - self.application_process = None - self.pause_sim() - self.reset_sim() + stop_process_and_children(process) except Exception: LogManager.logger.exception("No application running") print(traceback.format_exc()) + if len(self.application_processes) > 0: + self.pause_sim() + self.reset_sim() + self.application_processes = [] + def on_terminate_tools(self, event): self.tools_launcher.terminate() @@ -895,9 +927,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 launcher in self.robot_launchers: + launcher.terminate() + self.robot_launchers = [] def on_disconnect(self, event): """ @@ -907,12 +940,12 @@ def on_disconnect(self, event): terminates launchers, and restarts the script. """ - if self.application_process: - try: - stop_process_and_children(self.application_process) - self.application_process = None - except Exception as e: - LogManager.logger.exception("Exception stopping application process") + try: + for process in self.application_processes: + stop_process_and_children(process) + self.application_processes = [] + except Exception: + LogManager.logger.exception("Exception stopping application process") if self.tools_launcher: try: @@ -920,11 +953,12 @@ def on_disconnect(self, event): except Exception as e: LogManager.logger.exception("Exception terminating tools launcher") - if self.robot_launcher: - try: - self.robot_launcher.terminate() - except Exception as e: - LogManager.logger.exception("Exception terminating robot launcher") + try: + for launcher in self.robot_launchers: + launcher.terminate() + self.robot_launchers = [] + except Exception as e: + LogManager.logger.exception("Exception terminating robot launcher") if self.scene_launcher: try: @@ -943,15 +977,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 len(self.application_processes) > 0: + for process in self.application_processes: + proc = psutil.Process(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 +1002,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 len(self.application_processes) > 0: + for process in self.application_processes: + proc = psutil.Process(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 +1041,22 @@ 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 = [] + for robot_config in self.robot_configs: + entities.append(robot_config["entity"]) + 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: - try: - self.robot_launcher.run( - self.robot_config["entity"], - self.robot_config["start_pose"], - self.robot_config["extra_config"], - ) - except Exception as e: - LogManager.logger.exception("Exception terminating scene launcher") + robots_data = zip(self.robot_launchers, self.robot_configs) + for launcher, cfg in robots_data: + launcher.run(cfg["entity"], cfg["start_pose"], cfg["extra_config"]) def start(self): """ @@ -1049,14 +1080,12 @@ def signal_handler(sign, frame): except Exception as e: LogManager.logger.exception("Exception stopping consumer") - if self.application_process: - try: - stop_process_and_children(self.application_process) - self.application_process = None - except Exception as e: - LogManager.logger.exception( - "Exception stopping application process" - ) + try: + for process in self.application_processes: + stop_process_and_children(process) + self.application_processes = [] + except Exception: + LogManager.logger.exception("Exception stopping application process") if self.tools_launcher: try: @@ -1064,11 +1093,12 @@ def signal_handler(sign, frame): except Exception as e: LogManager.logger.exception("Exception terminating tools launcher") - if self.robot_launcher: - try: - self.robot_launcher.terminate() - except Exception as e: - LogManager.logger.exception("Exception terminating robot launcher") + try: + for launcher in self.robot_launchers: + launcher.terminate() + self.robot_launchers = [] + except Exception as e: + LogManager.logger.exception("Exception terminating robot launcher") if self.scene_launcher: try: