diff --git a/robotics_application_manager/manager/launcher/launcher_console.py b/robotics_application_manager/manager/launcher/launcher_console.py index af94190..fbbf1fc 100644 --- a/robotics_application_manager/manager/launcher/launcher_console.py +++ b/robotics_application_manager/manager/launcher/launcher_console.py @@ -45,7 +45,7 @@ def pause(self): def unpause(self): pass - def reset(self): + def reset(self, robot_entity=None): pass def is_running(self): diff --git a/robotics_application_manager/manager/launcher/launcher_gazebo.py b/robotics_application_manager/manager/launcher/launcher_gazebo.py index 3f81a1f..d85d424 100644 --- a/robotics_application_manager/manager/launcher/launcher_gazebo.py +++ b/robotics_application_manager/manager/launcher/launcher_gazebo.py @@ -1,9 +1,9 @@ """ Gazebo Classic Launcher and Lifecycle Manager. -Handles the initialization, state management, and cleanup of the -Gazebo GUI client (gzclient). Orchestrates display routing via -VNC and provides hardware-accelerated rendering through VirtualGL +Handles the initialization, state management, and cleanup of the +Gazebo GUI client (gzclient). Orchestrates display routing via +VNC and provides hardware-accelerated rendering through VirtualGL when compatible DRI devices are detected. """ @@ -106,7 +106,7 @@ def unpause(self): """Resumes the physics engine via the /unpause_physics ROS 2 service.""" call_service("/unpause_physics", "std_srvs/srv/Empty") - def reset(self): + def reset(self, robot_entity=None): call_service("/reset_world", "std_srvs/srv/Empty") 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 261b19a..4f03395 100644 --- a/robotics_application_manager/manager/launcher/launcher_gzsim.py +++ b/robotics_application_manager/manager/launcher/launcher_gzsim.py @@ -13,6 +13,12 @@ from typing import List, Any from robotics_application_manager import LogManager +from gz.msgs10.world_control_pb2 import WorldControl +from gz.msgs10.world_reset_pb2 import WorldReset +from gz.msgs10.entity_pb2 import Entity +from gz.msgs10.boolean_pb2 import Boolean +from gz.transport13 import Node + def call_gzservice(service, reqtype, reptype, timeout, req): command = f"gz service -s {service} --reqtype {reqtype} --reptype {reptype} --timeout {timeout} --req '{req}'" @@ -150,20 +156,32 @@ def unpause(self): "pause: false", ) - def reset(self): + def reset(self, robot_entity=None): if is_ros_service_available("/drone0/platform/state_machine/_reset"): call_service( "/drone0/platform/state_machine/_reset", "std_srvs/srv/Trigger", "{}", ) - call_gzservice( - "/world/default/control", - "gz.msgs.WorldControl", - "gz.msgs.Boolean", - "3000", - "reset: {all: true}", + node = Node() + + if robot_entity is not None: + node.request( + f"/world/default/remove", + Entity(name=robot_entity, type=Entity.MODEL), + Entity, + Boolean, + 1000, + ) + + node.request( + f"/world/default/control", + WorldControl(pause=True, reset=WorldReset(all=True)), + WorldControl, + Boolean, + 1000, ) + if is_ros_service_available("/drone0/controller/_reset"): call_service("/drone0/controller/_reset", "std_srvs/srv/Trigger", "{}") diff --git a/robotics_application_manager/manager/launcher/launcher_o3de.py b/robotics_application_manager/manager/launcher/launcher_o3de.py index 0bb9af1..289c148 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): + def reset(self, robot_entity=None): # TODO: add reset pass diff --git a/robotics_application_manager/manager/launcher/launcher_robot.py b/robotics_application_manager/manager/launcher/launcher_robot.py index 800cd97..20932ae 100644 --- a/robotics_application_manager/manager/launcher/launcher_robot.py +++ b/robotics_application_manager/manager/launcher/launcher_robot.py @@ -13,55 +13,20 @@ worlds = { "gazebo": { - "1": [ - { - "type": "module", - "module": "ros_api", - "parameters": [], - "launch_file": [], - } - ], "2": [ { - "type": "module", + "type": "gazebo", "module": "robot_ros2_api", "parameters": [], "launch_file": [], } ], }, - "drones": { - "1": [ - { - "type": "module", - "module": "drones", - "resource_folders": [], - "model_folders": [], - "plugin_folders": [], - "parameters": [], - "launch_file": [], - } - ], - "2": [ - { - "type": "module", - "module": "drones_ros2", - "resource_folders": [], - "model_folders": [], - "plugin_folders": [], - "parameters": [], - "launch_file": [], - } - ], - }, - "gzsimdrones": { + "gz": { "2": [ { - "type": "module", - "module": "drones_gzsim", - "resource_folders": [], - "model_folders": [], - "plugin_folders": [], + "type": "gz", + "module": "robot_ros2_api", "parameters": [], "launch_file": [], } @@ -81,13 +46,17 @@ class LauncherRobot(BaseModel): launchers: Optional[ILauncher] = [] start_pose: Optional[list] = [] - def run(self, start_pose=None): + def run(self, start_pose=None, extra_config=None): """Run the robot launcher with an optional start pose.""" if start_pose is not None: self.start_pose = start_pose + + if extra_config is None: + extra_config = "" + for module in worlds[self.type][str(self.ros_version)]: module["launch_file"] = self.launch_file_path - launcher = self.launch_module(module) + launcher = self.launch_module(module, extra_config) self.launchers.append(launcher) LogManager.logger.info(self.launchers) @@ -99,7 +68,7 @@ def terminate(self): launcher.terminate() self.launchers = [] - def launch_module(self, configuration): + def launch_module(self, configuration, extra_config=None): """Launch a robot module based on the provided configuration.""" def process_terminated(name, exit_code): @@ -117,7 +86,7 @@ def process_terminated(name, exit_code): launcher_class = get_class(launcher_module) launcher = launcher_class.from_config(launcher_class, configuration) - launcher.run(self.start_pose, process_terminated) + launcher.run(self.start_pose, extra_config, process_terminated) return launcher def launch_command(self, configuration): 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 eb25b48..0ef78ad 100644 --- a/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py +++ b/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py @@ -9,8 +9,31 @@ ) from robotics_application_manager.manager.docker_thread import DockerThread import subprocess +import sys import logging +from robotics_application_manager import LogManager + + +def call_service(service, service_type, request_data="{}"): + command = f"ros2 service call {service} {service_type} '{request_data}'" + try: + p = subprocess.Popen( + [ + f"{command}", + ], + shell=True, + stdout=sys.stdout, + stderr=subprocess.STDOUT, + bufsize=1024, + universal_newlines=True, + ) + p.wait(10) + except: + p.kill() + + LogManager.logger.exception(f"Unable to complete call: {service}") + raise Exception(f"Unable to complete call: {service}") class LauncherRobotRos2Api(ILauncher): @@ -19,7 +42,7 @@ class LauncherRobotRos2Api(ILauncher): launch_file: str threads: List[Any] = [] - def run(self, robot_pose, callback): + def run(self, robot_pose, extra_config, callback): DRI_PATH = self.get_dri_path() ACCELERATION_ENABLED = self.check_device(DRI_PATH) @@ -30,12 +53,15 @@ def run(self, robot_pose, callback): xserver_thread.start() self.threads.append(xserver_thread) - ROBOT_POSE = f"ROBOT_X={robot_pose[0]} ROBOT_Y={robot_pose[1]} ROBOT_Z={robot_pose[2]} ROBOT_ROLL={robot_pose[3]} ROBOT_PITCH={robot_pose[4]} ROBOT_YAW={robot_pose[5]}" + 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 {ROBOT_POSE} ros2 launch {self.launch_file}" + 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}" else: - exercise_launch_cmd = f"{ROBOT_POSE} ros2 launch {self.launch_file}" + 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_thread = DockerThread(exercise_launch_cmd) exercise_launch_thread.start() @@ -58,8 +84,7 @@ def terminate(self): universal_newlines=True, ) - kill_cmd = "pkill -9 " - cmd = kill_cmd + "bridg" + cmd = kill_cmd + "bridge" subprocess.call( cmd, shell=True, diff --git a/robotics_application_manager/manager/launcher/launcher_rviz.py b/robotics_application_manager/manager/launcher/launcher_rviz.py index e066aa2..193957e 100644 --- a/robotics_application_manager/manager/launcher/launcher_rviz.py +++ b/robotics_application_manager/manager/launcher/launcher_rviz.py @@ -68,7 +68,7 @@ def pause(self): def unpause(self): pass - def reset(self): + def reset(self, robot_entity=None): pass def is_running(self): diff --git a/robotics_application_manager/manager/launcher/launcher_state_monitor.py b/robotics_application_manager/manager/launcher/launcher_state_monitor.py index 2785fb8..7b4767d 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): + def reset(self, robot_entity=None): 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 19242fb..c7f5e0b 100644 --- a/robotics_application_manager/manager/launcher/launcher_tools.py +++ b/robotics_application_manager/manager/launcher/launcher_tools.py @@ -131,9 +131,9 @@ def unpause(self): for launcher in self.launchers: launcher.unpause() - def reset(self): + def reset(self, robot_entity=None): for launcher in self.launchers: - launcher.reset() + launcher.reset(robot_entity) 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 1e4f6db..a93e222 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): + def reset(self, robot_entity=None): pass def died(self): diff --git a/robotics_application_manager/manager/manager.py b/robotics_application_manager/manager/manager.py index 2e49bde..0f89f31 100644 --- a/robotics_application_manager/manager/manager.py +++ b/robotics_application_manager/manager/manager.py @@ -225,6 +225,7 @@ def __init__(self, host: str, port: int): self.world_launcher = None self.world_type = None self.robot_launcher = None + self.robot_config = None self.tools_launcher = None self.application_process = None self.running = True @@ -345,25 +346,24 @@ def on_launch_world(self, event): self.world_launcher = LauncherWorld(**cfg.model_dump()) LogManager.logger.info(str(self.world_launcher)) - self.world_launcher.run() - LogManager.logger.info("Launch transition finished") # Launch robot - try: - if robot_cfg["type"] == None: - self.robot_launcher = None - LogManager.logger.info("Launch transition finished") - return - cfg = ConfigurationManager.validate(robot_cfg) - LogManager.logger.info("Launching robot from the RB") + self.robot_launcher = None + if robot_cfg["type"] is not None: + 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}") - LogManager.logger.info(cfg) - 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)) - self.robot_launcher = LauncherRobot(**cfg.model_dump()) - LogManager.logger.info(str(self.robot_launcher)) - self.robot_launcher.run(robot_cfg["start_pose"]) + self.world_launcher.run() + if self.robot_launcher is not None: + self.robot_launcher.run(robot_cfg["start_pose"], robot_cfg["extra_config"]) LogManager.logger.info("Launch transition finished") def prepare_custom_universe(self, cfg_dict): @@ -995,14 +995,19 @@ def reset_sim(self): self.robot_launcher.terminate() try: - self.tools_launcher.reset() + entity = None + if self.robot_config is not None: + entity = self.robot_config["entity"] + self.tools_launcher.reset(entity) 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_launcher.run( + self.robot_config["start_pose"], self.robot_config["extra_config"] + ) except Exception as e: LogManager.logger.exception("Exception terminating world launcher")