diff --git a/robotics_application_manager/manager/launcher/launcher_console.py b/robotics_application_manager/manager/launcher/launcher_console.py index fbbf1fc..9c10460 100644 --- a/robotics_application_manager/manager/launcher/launcher_console.py +++ b/robotics_application_manager/manager/launcher/launcher_console.py @@ -52,8 +52,9 @@ def is_running(self): return self.running def terminate(self): + LogManager.logger.info(f"Terminating console tool") self.console_vnc.terminate() - for thread in self.threads: + for thread in self.threads[:]: if thread.is_alive(): thread.terminate() thread.join() diff --git a/robotics_application_manager/manager/launcher/launcher_gzsim.py b/robotics_application_manager/manager/launcher/launcher_gzsim.py index bddd677..3959531 100644 --- a/robotics_application_manager/manager/launcher/launcher_gzsim.py +++ b/robotics_application_manager/manager/launcher/launcher_gzsim.py @@ -20,62 +20,6 @@ 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}'" - 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}") - - -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}") - - -def is_ros_service_available(service_name): - try: - result = subprocess.run( - ["ros2", "service", "list", "--include-hidden-services"], - capture_output=True, - text=True, - check=True, - ) - return service_name in result.stdout - except subprocess.CalledProcessError as e: - LogManager.logger.exception(f"Error checking service availability: {e}") - return False - - class LauncherGzsim(ILauncher): display: str internal_port: int @@ -129,40 +73,37 @@ def is_running(self): return self.running def terminate(self): + LogManager.logger.info(f"Terminating gz tool") self.gz_vnc.terminate() - for thread in self.threads: - thread.terminate() - thread.join() + for thread in self.threads[:]: + if thread.is_alive(): + thread.terminate() + thread.join() + self.threads.remove(thread) self.running = False def died(self): pass def pause(self): - call_gzservice( - "/world/default/control", - "gz.msgs.WorldControl", - "gz.msgs.Boolean", - "3000", - "pause: true", + Node().request( + f"/world/default/control", + WorldControl(pause=True), + WorldControl, + Boolean, + 10000, ) def unpause(self): - call_gzservice( - "/world/default/control", - "gz.msgs.WorldControl", - "gz.msgs.Boolean", - "3000", - "pause: false", + Node().request( + f"/world/default/control", + WorldControl(pause=False), + WorldControl, + Boolean, + 10000, ) 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", - "{}", - ) node = Node() node.request( @@ -190,9 +131,6 @@ def reset(self, robot_entity=None): 10000, ) - if is_ros_service_available("/drone0/controller/_reset"): - call_service("/drone0/controller/_reset", "std_srvs/srv/Trigger", "{}") - def get_dri_path(self): directory_path = "/dev/dri" dri_path = "" diff --git a/robotics_application_manager/manager/launcher/launcher_robot.py b/robotics_application_manager/manager/launcher/launcher_robot.py index 4e35ce9..13f93fd 100644 --- a/robotics_application_manager/manager/launcher/launcher_robot.py +++ b/robotics_application_manager/manager/launcher/launcher_robot.py @@ -55,7 +55,7 @@ def run(self, entity="", start_pose=None, extra_config=None): def terminate(self): """Terminate all robot launchers and clear the launchers list.""" - LogManager.logger.info("Terminating robot launcher") + LogManager.logger.info("Terminating robots launchers") if self.launchers: for launcher in self.launchers: launcher.terminate() 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 a59b212..f47eea8 100644 --- a/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py +++ b/robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py @@ -31,11 +31,6 @@ def run(self, entity, robot_pose, extra_config, callback): 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) - x, y, z, R, P, Y = robot_pose if extra_config == "None": @@ -48,6 +43,7 @@ def run(self, entity, robot_pose, extra_config, callback): 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() @@ -67,37 +63,9 @@ def run(self, entity, robot_pose, extra_config, callback): LogManager.logger.info("Robot spawned OK") def terminate(self): - if self.threads is not None: - for thread in self.threads: - if thread.is_alive(): - thread.terminate() - thread.join() - self.threads.remove(thread) - - kill_cmd = "pkill -9 -f " - cmd = kill_cmd + "spawn_robot.launch.py" - subprocess.call( - cmd, - shell=True, - stdout=subprocess.PIPE, - bufsize=1024, - universal_newlines=True, - ) - - cmd = kill_cmd + "bridge" - subprocess.call( - cmd, - shell=True, - stdout=subprocess.PIPE, - bufsize=1024, - universal_newlines=True, - ) - - cmd = kill_cmd + "robot_state_publisher" - subprocess.call( - cmd, - shell=True, - stdout=subprocess.PIPE, - bufsize=1024, - universal_newlines=True, - ) + LogManager.logger.info(f"Terminating robot launcher") + for thread in self.threads[:]: + if thread.is_alive(): + thread.terminate() + thread.join() + self.threads.remove(thread) diff --git a/robotics_application_manager/manager/launcher/launcher_ros2_api.py b/robotics_application_manager/manager/launcher/launcher_ros2_api.py index 4840dce..d58fa41 100644 --- a/robotics_application_manager/manager/launcher/launcher_ros2_api.py +++ b/robotics_application_manager/manager/launcher/launcher_ros2_api.py @@ -10,6 +10,7 @@ ) from robotics_application_manager.manager.docker_thread import DockerThread import subprocess +from robotics_application_manager import LogManager import logging @@ -38,28 +39,12 @@ def run(self, callback): exercise_launch_thread = DockerThread(exercise_launch_cmd) exercise_launch_thread.start() + self.threads.append(exercise_launch_thread) def terminate(self): - if self.threads is not None: - for thread in self.threads: - if thread.is_alive(): - thread.terminate() - thread.join() - self.threads.remove(thread) - - to_kill = ["launch.py"] - if self.type == "gz": - to_kill = ["drones_ws", "gz", "launch.py"] - else: - to_kill = ["gzserver", "launch.py"] - - kill_cmd = "pkill -9 -f " - for i in to_kill: - cmd = kill_cmd + i - subprocess.call( - cmd, - shell=True, - stdout=subprocess.PIPE, - bufsize=1024, - universal_newlines=True, - ) + LogManager.logger.info(f"Terminating world launcher") + for thread in self.threads[:]: + if thread.is_alive(): + thread.terminate() + thread.join() + self.threads.remove(thread) diff --git a/robotics_application_manager/manager/launcher/launcher_rviz.py b/robotics_application_manager/manager/launcher/launcher_rviz.py index 193957e..e9d80c3 100644 --- a/robotics_application_manager/manager/launcher/launcher_rviz.py +++ b/robotics_application_manager/manager/launcher/launcher_rviz.py @@ -23,7 +23,7 @@ class LauncherRviz(ILauncher): running: bool = False acceptsMsgs: bool = False threads: List[Any] = [] - console_vnc: Any = Vnc_server() + vnc: Any = Vnc_server() def run(self, config_file, callback): """ @@ -42,24 +42,21 @@ def run(self, config_file, callback): if config_file != None: config = f"ros2 launch {config_file}" - print(config) if ACCELERATION_ENABLED: - self.console_vnc.start_vnc_gpu( + self.vnc.start_vnc_gpu( self.display, self.internal_port, self.external_port, DRI_PATH ) # Write display config and start the console - console_cmd = f"export VGL_DISPLAY={DRI_PATH}; export DISPLAY={self.display}; vglrun {config}" + rviz_cmd = f"export VGL_DISPLAY={DRI_PATH}; export DISPLAY={self.display}; vglrun {config}" else: - self.console_vnc.start_vnc( - self.display, self.internal_port, self.external_port - ) + self.vnc.start_vnc(self.display, self.internal_port, self.external_port) # Write display config and start the console - console_cmd = f"export DISPLAY={self.display};{config}" + rviz_cmd = f"export DISPLAY={self.display};{config}" - console_thread = DockerThread(console_cmd) - console_thread.start() - self.threads.append(console_thread) + rviz_thread = DockerThread(rviz_cmd) + rviz_thread.start() + self.threads.append(rviz_thread) self.running = True def pause(self): @@ -75,8 +72,9 @@ def is_running(self): return self.running def terminate(self): - self.console_vnc.terminate() - for thread in self.threads: + LogManager.logger.info(f"Terminating rviz tool") + self.vnc.terminate() + for thread in self.threads[:]: if thread.is_alive(): thread.terminate() thread.join() @@ -85,27 +83,3 @@ def terminate(self): def died(self): pass - - # rviz_node_full = Node( - # package="rviz2", - # executable="rviz2", - # name="rviz2", - # output="log", - # arguments=["-d", rviz_full_config], - # parameters=[ - # robot_description, - # robot_description_semantic, - # kinematics_yaml, - - # pilz_planning_pipeline_config, - - # joint_limits, - # pilz_cartesian_limits, - - # trajectory_execution, - # moveit_controllers, - # planning_scene_monitor_parameters, - # move_group_capabilities, - # {"use_sim_time": True}, - # ] - # ) diff --git a/robotics_application_manager/manager/launcher/launcher_rviz_ros2.py b/robotics_application_manager/manager/launcher/launcher_rviz_ros2.py deleted file mode 100755 index 97728ba..0000000 --- a/robotics_application_manager/manager/launcher/launcher_rviz_ros2.py +++ /dev/null @@ -1,46 +0,0 @@ -from .launcher_interface import ILauncher -from robotics_application_manager.manager.docker_thread import DockerThread -from robotics_application_manager.manager.vnc import Vnc_server -import os -import stat - - -class LauncherRvizRos2(ILauncher): - display: str - internal_port: str - external_port: str - running = False - threads = [] - - def run(self, callback): - DRI_PATH = self.get_dri_path() - ACCELERATION_ENABLED = self.check_device(DRI_PATH) - rviz_vnc = Vnc_server() - - if ACCELERATION_ENABLED: - rviz_vnc.start_vnc_gpu( - self.display, self.internal_port, self.external_port, DRI_PATH - ) - rviz_cmd = f"export DISPLAY={self.display}; export VGL_DISPLAY={DRI_PATH}; vglrun rviz2" - else: - rviz_vnc.start_vnc(self.display, self.internal_port, self.external_port) - rviz_cmd = f"export DISPLAY={self.display}; rviz2" - - rviz_thread = DockerThread(rviz_cmd) - rviz_thread.start() - self.threads.append(rviz_thread) - self.running = True - - def is_running(self): - return self.running - - def terminate(self): - for thread in self.threads: - if thread.is_alive(): - thread.terminate() - thread.join() - self.threads.remove(thread) - self.running = False - - def died(self): - pass diff --git a/robotics_application_manager/manager/launcher/launcher_tools.py b/robotics_application_manager/manager/launcher/launcher_tools.py index 8af645f..a294de1 100644 --- a/robotics_application_manager/manager/launcher/launcher_tools.py +++ b/robotics_application_manager/manager/launcher/launcher_tools.py @@ -84,7 +84,7 @@ def run(self, consumer): self.launchers.append(launcher) def terminate(self): - LogManager.logger.info("Terminating tools launcher") + LogManager.logger.info("Terminating tools launchers") for launcher in self.launchers: if launcher.is_running(): launcher.terminate() diff --git a/robotics_application_manager/manager/launcher/launcher_world.py b/robotics_application_manager/manager/launcher/launcher_world.py index a8104d7..0c54041 100644 --- a/robotics_application_manager/manager/launcher/launcher_world.py +++ b/robotics_application_manager/manager/launcher/launcher_world.py @@ -62,7 +62,7 @@ def run(self): self.launchers.append(launcher) def terminate(self): - LogManager.logger.info("Terminating world launcher") + LogManager.logger.info("Terminating worlds launchers") if self.launchers: for launcher in self.launchers: launcher.terminate() diff --git a/robotics_application_manager/manager/manager.py b/robotics_application_manager/manager/manager.py index d438046..7698fd6 100644 --- a/robotics_application_manager/manager/manager.py +++ b/robotics_application_manager/manager/manager.py @@ -925,7 +925,8 @@ def on_disconnect(self, event): def process_message(self, message): if message.command == "gui": - self.tools_launcher.pass_msg(message.data) + if self.tools_launcher is not None: + self.tools_launcher.pass_msg(message.data) return self.trigger(message.command, data=message.data or None)