From ee8e479c8e2076977e9ac75146b71c1e3b62e5a9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=93scar=20Mart=C3=ADnez?= Date: Sun, 24 Mar 2024 11:09:47 +0100 Subject: [PATCH 1/2] Support for new superthin template for follow_person --- .../libs/applications/compatibility/server.py | 12 +- manager/manager/launcher/launcher_ros2_api.py | 59 +++-- manager/manager/manager.py | 243 ++++++++++++------ 3 files changed, 213 insertions(+), 101 deletions(-) diff --git a/manager/libs/applications/compatibility/server.py b/manager/libs/applications/compatibility/server.py index 6cae64c..7d5bc96 100644 --- a/manager/libs/applications/compatibility/server.py +++ b/manager/libs/applications/compatibility/server.py @@ -7,10 +7,14 @@ class Server(threading.Thread): - def __init__(self, port, callback,): + def __init__( + self, + port, + callback, + ): super().__init__() self.update_callback = callback - self.server = WebsocketServer(port=port, host='127.0.0.1') + self.server = WebsocketServer(port=port, host="127.0.0.1") self.server.set_fn_new_client(self.on_open) self.server.set_fn_client_left(self.on_close) self.server.set_fn_message_received(self.on_message) @@ -39,9 +43,7 @@ def send(self, data): def on_message(self, client, server, message): payload = json.loads(message) self.update_callback(payload) - server.send_message(client, "#ack") - LogManager.logger.debug( - f"Message received from template: {message[:30]}") + LogManager.logger.debug(f"Message received from template: {message[:30]}") def on_close(self, client, server): LogManager.logger.info("Connection with client closed") diff --git a/manager/manager/launcher/launcher_ros2_api.py b/manager/manager/launcher/launcher_ros2_api.py index d614c0c..2e740b4 100644 --- a/manager/manager/launcher/launcher_ros2_api.py +++ b/manager/manager/launcher/launcher_ros2_api.py @@ -9,13 +9,14 @@ import logging + class LauncherRos2Api(ILauncher): type: str module: str launch_file: str threads: List[Any] = [] - def run(self,callback): + def run(self, callback): DRI_PATH = os.path.join("/dev/dri", os.environ.get("DRI_NAME", "card0")) ACCELERATION_ENABLED = self.check_device(DRI_PATH) @@ -26,14 +27,16 @@ def run(self,callback): xserver_thread.start() self.threads.append(xserver_thread) - if (ACCELERATION_ENABLED): - exercise_launch_cmd = f"export VGL_DISPLAY={DRI_PATH}; vglrun ros2 launch {self.launch_file}" + if ACCELERATION_ENABLED: + exercise_launch_cmd = ( + f"export VGL_DISPLAY={DRI_PATH}; vglrun ros2 launch {self.launch_file}" + ) else: exercise_launch_cmd = f"ros2 launch {self.launch_file}" exercise_launch_thread = DockerThread(exercise_launch_cmd) exercise_launch_thread.start() - + def check_device(self, device_path): try: return stat.S_ISCHR(os.lstat(device_path)[stat.ST_MODE]) @@ -45,15 +48,39 @@ def terminate(self): for thread in self.threads: thread.terminate() thread.join() - - kill_cmd = 'pkill -9 -f ' - cmd = kill_cmd + 'gzserver' - subprocess.call(cmd, shell=True, stdout=subprocess.PIPE, bufsize=1024, universal_newlines=True) - cmd = kill_cmd + 'spawn_model.launch.py' - subprocess.call(cmd, shell=True, stdout=subprocess.PIPE, bufsize=1024, universal_newlines=True) - - kill_cmd = 'pkill -9 -f ' - cmd = kill_cmd + 'gzserver' - subprocess.call(cmd, shell=True, stdout=subprocess.PIPE, bufsize=1024, universal_newlines=True) - cmd = kill_cmd + 'spawn_model.launch.py' - subprocess.call(cmd, shell=True, stdout=subprocess.PIPE, bufsize=1024, universal_newlines=True) + + kill_cmd = "pkill -9 -f " + cmd = kill_cmd + "gzserver" + subprocess.call( + cmd, + shell=True, + stdout=subprocess.PIPE, + bufsize=1024, + universal_newlines=True, + ) + cmd = kill_cmd + "spawn_model.launch.py" + subprocess.call( + cmd, + shell=True, + stdout=subprocess.PIPE, + bufsize=1024, + universal_newlines=True, + ) + + kill_cmd = "pkill -9 -f " + cmd = kill_cmd + "gzserver" + subprocess.call( + cmd, + shell=True, + stdout=subprocess.PIPE, + bufsize=1024, + universal_newlines=True, + ) + cmd = kill_cmd + "spawn_model.launch.py" + subprocess.call( + cmd, + shell=True, + stdout=subprocess.PIPE, + bufsize=1024, + universal_newlines=True, + ) diff --git a/manager/manager/manager.py b/manager/manager/manager.py index cb79523..2e198c5 100644 --- a/manager/manager/manager.py +++ b/manager/manager/manager.py @@ -8,7 +8,8 @@ import psutil import shutil import time -if ("noetic" in str(subprocess.check_output(['bash', '-c', 'echo $ROS_DISTRO']))): + +if "noetic" in str(subprocess.check_output(["bash", "-c", "echo $ROS_DISTRO"])): import rosservice import traceback from queue import Queue @@ -25,7 +26,9 @@ from src.manager.manager.launcher.launcher_visualization import LauncherVisualization from src.manager.ram_logging.log_manager import LogManager from src.manager.libs.applications.compatibility.server import Server -from src.manager.manager.application.robotics_python_application_interface import IRoboticsPythonApplication +from src.manager.manager.application.robotics_python_application_interface import ( + IRoboticsPythonApplication, +) from src.manager.libs.process_utils import stop_process_and_children from src.manager.manager.lint.linter import Lint @@ -37,51 +40,90 @@ class Manager: "world_ready", "visualization_ready", "application_running", - "paused" + "paused", ] transitions = [ # Transitions for state idle - {'trigger': 'connect', 'source': 'idle', - 'dest': 'connected', 'before': 'on_connect'}, - + { + "trigger": "connect", + "source": "idle", + "dest": "connected", + "before": "on_connect", + }, # Transitions for state connected - {'trigger': 'launch_world', 'source': 'connected', - 'dest': 'world_ready', 'before': 'on_launch_world'}, - + { + "trigger": "launch_world", + "source": "connected", + "dest": "world_ready", + "before": "on_launch_world", + }, # Transitions for state world ready - {'trigger': 'prepare_visualization', - 'source': 'world_ready', 'dest': 'visualization_ready', 'before': 'on_prepare_visualization'}, - + { + "trigger": "prepare_visualization", + "source": "world_ready", + "dest": "visualization_ready", + "before": "on_prepare_visualization", + }, # Transitions for state visualization_ready - {'trigger': 'run_application', 'source': [ - 'visualization_ready', 'paused'], 'dest': 'application_running', 'before': 'on_run_application'}, - + { + "trigger": "run_application", + "source": ["visualization_ready", "paused"], + "dest": "application_running", + "before": "on_run_application", + }, # Transitions for state application_running - {'trigger': 'pause', 'source': 'application_running', - 'dest': 'paused', 'before': 'on_pause'}, - {'trigger': 'resume', 'source': 'paused', - 'dest': 'application_running', 'before': 'on_resume'}, - + { + "trigger": "pause", + "source": "application_running", + "dest": "paused", + "before": "on_pause", + }, + { + "trigger": "resume", + "source": "paused", + "dest": "application_running", + "before": "on_resume", + }, # Transitions for terminate levels - {'trigger': 'terminate_application', 'source': ['visualization_ready','application_running', 'paused'], - 'dest': 'visualization_ready', 'before': 'on_terminate_application'}, - {'trigger': 'terminate_visualization', 'source': 'visualization_ready', - 'dest': 'world_ready', 'before': 'on_terminate_visualization'}, - {'trigger': 'terminate_universe', 'source': 'world_ready', - 'dest': 'connected', 'before': 'on_terminate_universe'}, - + { + "trigger": "terminate_application", + "source": ["visualization_ready", "application_running", "paused"], + "dest": "visualization_ready", + "before": "on_terminate_application", + }, + { + "trigger": "terminate_visualization", + "source": "visualization_ready", + "dest": "world_ready", + "before": "on_terminate_visualization", + }, + { + "trigger": "terminate_universe", + "source": "world_ready", + "dest": "connected", + "before": "on_terminate_universe", + }, # Global transitions - {'trigger': 'disconnect', 'source': '*', - 'dest': 'idle', 'before': 'on_disconnect'}, + { + "trigger": "disconnect", + "source": "*", + "dest": "idle", + "before": "on_disconnect", + }, ] def __init__(self, host: str, port: int): - self.machine = Machine(model=self, states=Manager.states, transitions=Manager.transitions, - initial='idle', send_event=True, after_state_change=self.state_change) - self.ros_version = subprocess.check_output( - ['bash', '-c', 'echo $ROS_DISTRO']) + self.machine = Machine( + model=self, + states=Manager.states, + transitions=Manager.transitions, + initial="idle", + send_event=True, + after_state_change=self.state_change, + ) + self.ros_version = subprocess.check_output(["bash", "-c", "echo $ROS_DISTRO"]) self.queue = Queue() self.consumer = ManagerConsumer(host, port, self.queue) self.world_launcher = None @@ -105,13 +147,12 @@ def __init__(self, host: str, port: int): def state_change(self, event): LogManager.logger.info(f"State changed to {self.state}") if self.consumer is not None: - self.consumer.send_message( - {'state': self.state}, command="state-changed") + self.consumer.send_message({"state": self.state}, command="state-changed") def update(self, data): LogManager.logger.debug(f"Sending update to client") if self.consumer is not None: - self.consumer.send_message({'update': data}, command="update") + self.consumer.send_message({"update": data}, command="update") def on_connect(self, event): """ @@ -126,24 +167,34 @@ def on_connect(self, event): - `ros_version`: The current ROS (Robot Operating System) distribution version. - `gpu_avaliable`: Boolean indicating whether GPU acceleration is available. """ - self.consumer.send_message({'radi_version': subprocess.check_output(['bash', '-c', 'echo $IMAGE_TAG']), 'ros_version': subprocess.check_output( - ['bash', '-c', 'echo $ROS_DISTRO']), 'gpu_avaliable': check_gpu_acceleration(), }, command="introspection") + self.consumer.send_message( + { + "radi_version": subprocess.check_output( + ["bash", "-c", "echo $IMAGE_TAG"] + ), + "ros_version": subprocess.check_output( + ["bash", "-c", "echo $ROS_DISTRO"] + ), + "gpu_avaliable": check_gpu_acceleration(), + }, + command="introspection", + ) def on_launch_world(self, event): """ - Handles the 'launch' event, transitioning the application from 'connected' to 'ready' state. + Handles the 'launch' event, transitioning the application from 'connected' to 'ready' state. This method initializes the launch process based on the provided configuration. - During the launch process, it validates and processes the configuration data received from the event. - It then creates and starts a LauncherWorld instance with the validated configuration. + During the launch process, it validates and processes the configuration data received from the event. + It then creates and starts a LauncherWorld instance with the validated configuration. This setup is crucial for preparing the environment and resources necessary for the application's execution. Parameters: - event (Event): The event object containing data related to the 'launch' event. + event (Event): The event object containing data related to the 'launch' event. This data includes configuration information necessary for initializing the launch process. Raises: - ValueError: If the configuration data is invalid or incomplete, a ValueError is raised, + ValueError: If the configuration data is invalid or incomplete, a ValueError is raised, indicating the issue with the provided configuration. Note: @@ -151,10 +202,10 @@ def on_launch_world(self, event): """ try: - config_dict = event.kwargs.get('data', {}) + config_dict = event.kwargs.get("data", {}) configuration = ConfigurationManager.validate(config_dict) except ValueError as e: - LogManager.logger.error(f'Configuration validotion failed: {e}') + LogManager.logger.error(f"Configuration validotion failed: {e}") self.world_launcher = LauncherWorld(**configuration.model_dump()) self.world_launcher.run() @@ -163,9 +214,10 @@ def on_launch_world(self, event): def on_prepare_visualization(self, event): LogManager.logger.info("Visualization transition started") - visualization_type = event.kwargs.get('data', {}) + visualization_type = event.kwargs.get("data", {}) self.visualization_launcher = LauncherVisualization( - visualization=visualization_type) + visualization=visualization_type + ) self.visualization_launcher.run() if visualization_type == "gazebo_rae": @@ -181,11 +233,17 @@ def add_frequency_control(self, code): """ code = frequency_control_code_imports + code infinite_loop = re.search( - r'[^ ]while\s*\(\s*True\s*\)\s*:|[^ ]while\s*True\s*:|[^ ]while\s*1\s*:|[^ ]while\s*\(\s*1\s*\)\s*:', code) + r"[^ ]while\s*\(\s*True\s*\)\s*:|[^ ]while\s*True\s*:|[^ ]while\s*1\s*:|[^ ]while\s*\(\s*1\s*\)\s*:", + code, + ) frequency_control_code_pre = """ start_time = datetime.now() """ - code = code[:infinite_loop.end()] + frequency_control_code_pre + code[infinite_loop.end():] + code = ( + code[: infinite_loop.end()] + + frequency_control_code_pre + + code[infinite_loop.end() :] + ) frequency_control_code_post = """ finish_time = datetime.now() dt = finish_time - start_time @@ -201,18 +259,18 @@ def on_run_application(self, event): superthin = False # Extract app config - application_configuration = event.kwargs.get('data', {}) - application_file_path = application_configuration['template'] - exercise_id = application_configuration['exercise_id'] - code = application_configuration['code'] + application_configuration = event.kwargs.get("data", {}) + application_file_path = application_configuration["template"] + exercise_id = application_configuration["exercise_id"] + code = application_configuration["code"] # Template version if "noetic" in str(self.ros_version): - application_folder = application_file_path + '/ros1_noetic/' + application_folder = application_file_path + "/ros1_noetic/" else: - application_folder = application_file_path + '/ros2_humble/' + application_folder = application_file_path + "/ros2_humble/" - if not os.path.isfile(application_folder + 'exercise.py'): + if not os.path.isfile(application_folder + "exercise.py"): superthin = True # Create executable app @@ -226,18 +284,28 @@ def on_run_application(self, event): shutil.copytree(application_folder, "/workspace/code", dirs_exist_ok=True) if superthin: - self.application_process = subprocess.Popen(["python3", "/workspace/code/academy.py"], stdout=sys.stdout, stderr=subprocess.STDOUT, - bufsize=1024, universal_newlines=True) + self.application_process = subprocess.Popen( + ["python3", "/workspace/code/academy.py"], + stdout=sys.stdout, + stderr=subprocess.STDOUT, + bufsize=1024, + universal_newlines=True, + ) else: - self.application_process = subprocess.Popen(["python3", "/workspace/code/exercise.py"], stdout=sys.stdout, stderr=subprocess.STDOUT, - bufsize=1024, universal_newlines=True) + self.application_process = subprocess.Popen( + ["python3", "/workspace/code/exercise.py"], + stdout=sys.stdout, + stderr=subprocess.STDOUT, + bufsize=1024, + universal_newlines=True, + ) self.unpause_sim() else: - print('errors') + print("errors") raise Exception(errors) - - LogManager.logger.info("Run application transition finished") - + + LogManager.logger.info("Run application transition finished") + def on_terminate_application(self, event): if self.application_process: @@ -277,7 +345,9 @@ def on_disconnect(self, event): try: self.visualization_launcher.terminate() except Exception as e: - LogManager.logger.exception("Exception terminating visualization launcher") + LogManager.logger.exception( + "Exception terminating visualization launcher" + ) if self.world_launcher: try: @@ -290,10 +360,10 @@ def on_disconnect(self, event): os.execl(python, python, *sys.argv) def process_messsage(self, message): - if message.command == "#gui": + if message.command == "gui": self.gui_server.send(message.data) return - + self.trigger(message.command, data=message.data or None) response = {"message": f"Exercise state changed to {self.state}"} self.consumer.send_message(message.response(response)) @@ -312,24 +382,30 @@ def pause_sim(self): if "noetic" in str(self.ros_version): rosservice.call_service("/gazebo/pause_physics", []) else: - self.call_service("/pause_physics","std_srvs/srv/Empty") + self.call_service("/pause_physics", "std_srvs/srv/Empty") def unpause_sim(self): if "noetic" in str(self.ros_version): rosservice.call_service("/gazebo/unpause_physics", []) else: - self.call_service("/unpause_physics","std_srvs/srv/Empty") + self.call_service("/unpause_physics", "std_srvs/srv/Empty") def reset_sim(self): if "noetic" in str(self.ros_version): rosservice.call_service("/gazebo/reset_world", []) else: - self.call_service("/reset_world","std_srvs/srv/Empty") + self.call_service("/reset_world", "std_srvs/srv/Empty") def call_service(self, service, service_type): command = f"ros2 service call {service} {service_type}" - subprocess.call(f"{command}", shell=True, stdout=sys.stdout, stderr=subprocess.STDOUT, bufsize=1024, - universal_newlines=True) + subprocess.call( + f"{command}", + shell=True, + stdout=sys.stdout, + stderr=subprocess.STDOUT, + bufsize=1024, + universal_newlines=True, + ) def start(self): """ @@ -337,7 +413,8 @@ def start(self): RAM must be run in main thread to be able to handle signaling other processes, for instance ROS launcher. """ LogManager.logger.info( - f"Starting RAM consumer in {self.consumer.server}:{self.consumer.port}") + f"Starting RAM consumer in {self.consumer.server}:{self.consumer.port}" + ) self.consumer.start() @@ -359,13 +436,17 @@ def signal_handler(sign, frame): stop_process_and_children(self.application_process) self.application_process = None except Exception as e: - LogManager.logger.exception("Exception stopping application process") + LogManager.logger.exception( + "Exception stopping application process" + ) if self.visualization_launcher: try: self.visualization_launcher.terminate() except Exception as e: - LogManager.logger.exception("Exception terminating visualization launcher") + LogManager.logger.exception( + "Exception terminating visualization launcher" + ) if self.world_launcher: try: @@ -373,7 +454,6 @@ def signal_handler(sign, frame): except Exception as e: LogManager.logger.exception("Exception terminating world launcher") - signal.signal(signal.SIGINT, signal_handler) while self.running: @@ -386,19 +466,22 @@ def signal_handler(sign, frame): self.process_messsage(message) except Exception as e: if message is not None: - ex = ManagerConsumerMessageException( - id=message.id, message=str(e)) + ex = ManagerConsumerMessageException(id=message.id, message=str(e)) else: ex = ManagerConsumerMessageException( - id=str(uuid4()), message=str(e)) + id=str(uuid4()), message=str(e) + ) self.consumer.send_message(ex) LogManager.logger.error(e, exc_info=True) + if __name__ == "__main__": import argparse + parser = argparse.ArgumentParser() parser.add_argument( - "host", type=str, help="Host to listen to (0.0.0.0 or all hosts)") + "host", type=str, help="Host to listen to (0.0.0.0 or all hosts)" + ) parser.add_argument("port", type=int, help="Port to listen to") args = parser.parse_args() From f5b3800f0c882573f06ae9191737b790d2914549 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=93scar=20Mart=C3=ADnez?= Date: Sun, 24 Mar 2024 11:34:15 +0100 Subject: [PATCH 2/2] New code style and CPU proper waiting --- manager/manager/launcher/launcher_console.py | 20 ++- .../manager/launcher/launcher_gazebo_view.py | 27 ++- .../launcher/launcher_visualization.py | 155 ++++++++++-------- manager/manager/launcher/launcher_world.py | 79 +++++---- manager/manager/vnc/vnc_server.py | 47 +++--- 5 files changed, 183 insertions(+), 145 deletions(-) diff --git a/manager/manager/launcher/launcher_console.py b/manager/manager/launcher/launcher_console.py index 1a5a888..1e642fe 100644 --- a/manager/manager/launcher/launcher_console.py +++ b/manager/manager/launcher/launcher_console.py @@ -1,7 +1,7 @@ from src.manager.manager.launcher.launcher_interface import ILauncher from src.manager.manager.docker_thread.docker_thread import DockerThread from src.manager.manager.vnc.vnc_server import Vnc_server -from src.manager.libs.process_utils import check_gpu_acceleration +from src.manager.libs.process_utils import check_gpu_acceleration import os import stat from typing import List, Any @@ -11,7 +11,7 @@ class LauncherConsole(ILauncher): display: str internal_port: int external_port: int - running:bool = False + running: bool = False threads: List[Any] = [] console_vnc: Any = Vnc_server() @@ -19,14 +19,16 @@ def run(self, callback): DRI_PATH = os.path.join("/dev/dri", os.environ.get("DRI_NAME", "card0")) ACCELERATION_ENABLED = False - - - if (ACCELERATION_ENABLED): - self.console_vnc.start_vnc_gpu(self.display, self.internal_port, self.external_port,DRI_PATH) + if ACCELERATION_ENABLED: + self.console_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 xterm -fullscreen -sb -fa 'Monospace' -fs 10 -bg black -fg white" else: - self.console_vnc.start_vnc(self.display, self.internal_port, self.external_port) + self.console_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};xterm -geometry 100x10+0+0 -fa 'Monospace' -fs 10 -bg black -fg white" @@ -35,7 +37,7 @@ def run(self, callback): self.threads.append(console_thread) self.running = True - + def check_device(self, device_path): try: return stat.S_ISCHR(os.lstat(device_path)[stat.ST_MODE]) @@ -53,4 +55,4 @@ def terminate(self): self.running = False def died(self): - pass \ No newline at end of file + pass diff --git a/manager/manager/launcher/launcher_gazebo_view.py b/manager/manager/launcher/launcher_gazebo_view.py index f1b79dd..12e5c59 100644 --- a/manager/manager/launcher/launcher_gazebo_view.py +++ b/manager/manager/launcher/launcher_gazebo_view.py @@ -1,7 +1,10 @@ from src.manager.manager.launcher.launcher_interface import ILauncher from src.manager.manager.docker_thread.docker_thread import DockerThread from src.manager.manager.vnc.vnc_server import Vnc_server -from src.manager.libs.process_utils import wait_for_process_to_start, check_gpu_acceleration +from src.manager.libs.process_utils import ( + wait_for_process_to_start, + check_gpu_acceleration, +) import subprocess import time import os @@ -29,17 +32,15 @@ def run(self, callback): if ACCELERATION_ENABLED: # Starts xserver, x11vnc and novnc self.gz_vnc.start_vnc_gpu( - self.display, self.internal_port, self.external_port, DRI_PATH) + self.display, self.internal_port, self.external_port, DRI_PATH + ) # Write display config and start gzclient - gzclient_cmd = ( - f"export DISPLAY={self.display}; {gzclient_config_cmds} export VGL_DISPLAY={DRI_PATH}; vglrun gzclient --verbose") + gzclient_cmd = f"export DISPLAY={self.display}; {gzclient_config_cmds} export VGL_DISPLAY={DRI_PATH}; vglrun gzclient --verbose" else: # Starts xserver, x11vnc and novnc - self.gz_vnc.start_vnc( - self.display, self.internal_port, self.external_port) + self.gz_vnc.start_vnc(self.display, self.internal_port, self.external_port) # Write display config and start gzclient - gzclient_cmd = ( - f"export DISPLAY={self.display}; {gzclient_config_cmds} gzclient --verbose") + gzclient_cmd = f"export DISPLAY={self.display}; {gzclient_config_cmds} gzclient --verbose" gzclient_thread = DockerThread(gzclient_cmd) gzclient_thread.start() @@ -70,14 +71,12 @@ def died(self): pass def get_dri_path(self): - directory_path = '/dev/dri' + directory_path = "/dev/dri" dri_path = "" if os.path.exists(directory_path) and os.path.isdir(directory_path): files = os.listdir(directory_path) - if ("card1" in files): - dri_path = os.path.join( - "/dev/dri", os.environ.get("DRI_NAME", "card1")) + if "card1" in files: + dri_path = os.path.join("/dev/dri", os.environ.get("DRI_NAME", "card1")) else: - dri_path = os.path.join( - "/dev/dri", os.environ.get("DRI_NAME", "card0")) + dri_path = os.path.join("/dev/dri", os.environ.get("DRI_NAME", "card0")) return dri_path diff --git a/manager/manager/launcher/launcher_visualization.py b/manager/manager/launcher/launcher_visualization.py index e1f0842..9f9b38c 100644 --- a/manager/manager/launcher/launcher_visualization.py +++ b/manager/manager/launcher/launcher_visualization.py @@ -10,80 +10,98 @@ visualization = { "none": [], - "console": [{"module": "console", - "display": ":1", - "external_port": 1108, - "internal_port": 5901}], - "gazebo_gra": [{ - "type": "module", - "module": "console", - "display": ":1", - "external_port": 1108, - "internal_port": 5901}, + "console": [ { - "type": "module", - "width": 1024, - "height": 768, - "module": "gazebo_view", - "display": ":2", - "external_port": 6080, - "internal_port": 5900 - }, + "module": "console", + "display": ":1", + "external_port": 1108, + "internal_port": 5901, + } + ], + "gazebo_gra": [ + { + "type": "module", + "module": "console", + "display": ":1", + "external_port": 1108, + "internal_port": 5901, + }, + { + "type": "module", + "width": 1024, + "height": 768, + "module": "gazebo_view", + "display": ":2", + "external_port": 6080, + "internal_port": 5900, + }, + { + "type": "module", + "width": 1024, + "height": 768, + "module": "robot_display_view", + "display": ":3", + "external_port": 2303, + "internal_port": 5902, + }, + ], + "gazebo_rae": [ + { + "type": "module", + "module": "console", + "display": ":1", + "external_port": 1108, + "internal_port": 5901, + }, + { + "type": "module", + "width": 1024, + "height": 768, + "module": "gazebo_view", + "display": ":2", + "external_port": 6080, + "internal_port": 5900, + }, + ], + "physic_gra": [ + { + "module": "console", + "display": ":1", + "external_port": 1108, + "internal_port": 5901, + }, + { + "type": "module", + "width": 1024, + "height": 768, + "module": "robot_display_view", + "display": ":2", + "external_port": 2303, + "internal_port": 5902, + }, + ], + "physic_rae": [ + { + "module": "console", + "display": ":1", + "external_port": 1108, + "internal_port": 5901, + }, { - "type": "module", - "width": 1024, - "height": 768, - "module": "robot_display_view", - "display": ":3", - "external_port": 2303, - "internal_port": 5902 - } + "type": "module", + "width": 1024, + "height": 768, + "module": "robot_display_view", + "display": ":2", + "external_port": 2303, + "internal_port": 5902, + }, ], - "gazebo_rae": [{"type": "module", - "module": "console", - "display": ":1", - "external_port": 1108, - "internal_port": 5901}, - { - "type": "module", - "width": 1024, - "height": 768, - "module": "gazebo_view", - "display": ":2", - "external_port": 6080, - "internal_port": 5900 - }], - "physic_gra": [{"module": "console", - "display": ":1", - "external_port": 1108, - "internal_port": 5901}, - { - "type": "module", - "width": 1024, - "height": 768, - "module": "robot_display_view", - "display": ":2", - "external_port": 2303, - "internal_port": 5902 - }], - "physic_rae": [{"module": "console", - "display": ":1", - "external_port": 1108, - "internal_port": 5901}, - { - "type": "module", - "width": 1024, - "height": 768, - "module": "robot_display_view", - "display": ":2", - "external_port": 2303, - "internal_port": 5902 - }] } class LauncherVisualization(BaseModel): - module: str = '.'.join(__name__.split('.')[:-1]) + module: str = ".".join(__name__.split(".")[:-1]) visualization: str launchers: Optional[ILauncher] = [] @@ -102,7 +120,8 @@ def terminate(self): def launch_module(self, configuration): def process_terminated(name, exit_code): LogManager.logger.info( - f"LauncherEngine: {name} exited with code {exit_code}") + f"LauncherEngine: {name} exited with code {exit_code}" + ) if self.terminated_callback is not None: self.terminated_callback(name, exit_code) diff --git a/manager/manager/launcher/launcher_world.py b/manager/manager/launcher/launcher_world.py index f1cf7b1..8e761cc 100644 --- a/manager/manager/launcher/launcher_world.py +++ b/manager/manager/launcher/launcher_world.py @@ -6,44 +6,56 @@ from src.manager.manager.launcher.launcher_interface import ILauncher worlds = { - "gazebo": - {"1": [{ - "type": "module", - "module": "ros_api", - "parameters": [], - "launch_file": [], - }], "2": [{ - "type": "module", - "module": "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": [], - }]}, - "physical": {} + "gazebo": { + "1": [ + { + "type": "module", + "module": "ros_api", + "parameters": [], + "launch_file": [], + } + ], + "2": [ + { + "type": "module", + "module": "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": [], + } + ], + }, + "physical": {}, } class LauncherWorld(BaseModel): world: str launch_file_path: str - module: str = '.'.join(__name__.split('.')[:-1]) + module: str = ".".join(__name__.split(".")[:-1]) ros_version: int = get_ros_version() launchers: Optional[ILauncher] = [] @@ -63,7 +75,8 @@ def terminate(self): def launch_module(self, configuration): def process_terminated(name, exit_code): LogManager.logger.info( - f"LauncherEngine: {name} exited with code {exit_code}") + f"LauncherEngine: {name} exited with code {exit_code}" + ) if self.terminated_callback is not None: self.terminated_callback(name, exit_code) diff --git a/manager/manager/vnc/vnc_server.py b/manager/manager/vnc/vnc_server.py index 8f0f59b..f39b124 100755 --- a/manager/manager/vnc/vnc_server.py +++ b/manager/manager/vnc/vnc_server.py @@ -6,6 +6,7 @@ import os from src.manager.libs.process_utils import wait_for_xserver + class Vnc_server: threads: List[Any] = [] running: bool = False @@ -25,7 +26,7 @@ def start_vnc(self, display, internal_port, external_port): self.threads.append(x11vnc_thread) # Start noVNC with default port 6080 listening to VNC server on 5900 - if self.get_ros_version() == '2': + if self.get_ros_version() == "2": novnc_cmd = f"/noVNC/utils/novnc_proxy --listen {external_port} --vnc localhost:{internal_port}" else: novnc_cmd = f"/noVNC/utils/launch.sh --listen {external_port} --vnc localhost:{internal_port}" @@ -36,8 +37,9 @@ def start_vnc(self, display, internal_port, external_port): self.running = True self.wait_for_port("localhost", internal_port) + self.wait_for_port("localhost", external_port) - def start_vnc_gpu(self,display, internal_port, external_port, dri_path): + def start_vnc_gpu(self, display, internal_port, external_port, dri_path): # Start X and VNC servers turbovnc_cmd = f"export VGL_DISPLAY={dri_path} && export TVNC_WM=startlxde && /opt/TurboVNC/bin/vncserver {display} -geometry '1920x1080' -vgl -noreset -SecurityTypes None -rfbport {internal_port}" turbovnc_thread = DockerThread(turbovnc_cmd) @@ -46,7 +48,7 @@ def start_vnc_gpu(self,display, internal_port, external_port, dri_path): wait_for_xserver(display) # Start noVNC with default port 6080 listening to VNC server on 5900 - if self.get_ros_version() == '2': + if self.get_ros_version() == "2": novnc_cmd = f"/noVNC/utils/novnc_proxy --listen {external_port} --vnc localhost:{internal_port}" else: novnc_cmd = f"/noVNC/utils/launch.sh --listen {external_port} --vnc localhost:{internal_port}" @@ -62,12 +64,13 @@ def start_vnc_gpu(self,display, internal_port, external_port, dri_path): self.create_desktop_icon() self.create_gzclient_icon() - def wait_for_port(self, host, port, timeout=20): start_time = time.time() while True: if time.time() - start_time > timeout: - raise TimeoutError(f"Port {port} on {host} didn't become available within {timeout} seconds.") + raise TimeoutError( + f"Port {port} on {host} didn't become available within {timeout} seconds." + ) try: with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock: sock.settimeout(1) @@ -79,53 +82,55 @@ def wait_for_port(self, host, port, timeout=20): def is_running(self): return self.running - def terminate(self): for thread in self.threads: thread.terminate() thread.join() self.running = False - def get_ros_version(self): - output = subprocess.check_output(['bash', '-c', 'echo $ROS_VERSION']) - return output.decode('utf-8').strip() - + output = subprocess.check_output(["bash", "-c", "echo $ROS_VERSION"]) + return output.decode("utf-8").strip() + def create_desktop_icon(self): try: - desktop_dir = os.path.expanduser('~/Desktop') + desktop_dir = os.path.expanduser("~/Desktop") if not os.path.exists(desktop_dir): - os.makedirs(desktop_dir) - desktop_path = os.path.join(desktop_dir, 'terminal_launcher.desktop') - with open(desktop_path, 'w') as f: - f.write("""[Desktop Entry] + os.makedirs(desktop_dir) + desktop_path = os.path.join(desktop_dir, "terminal_launcher.desktop") + with open(desktop_path, "w") as f: + f.write( + """[Desktop Entry] Name=Open Terminal Exec=xterm Icon=utilities-terminal Type=Application Encoding=UTF-8 Terminal=false - Categories=None;""") + Categories=None;""" + ) os.chmod(desktop_path, 0o755) except Exception as err: print(err) def create_gzclient_icon(self): - desktop_dir = os.path.expanduser('~/Desktop') + desktop_dir = os.path.expanduser("~/Desktop") if not os.path.exists(desktop_dir): os.makedirs(desktop_dir) - desktop_path = os.path.join(desktop_dir, 'gzclient_launcher.desktop') + desktop_path = os.path.join(desktop_dir, "gzclient_launcher.desktop") try: - with open(desktop_path, 'w') as f: - f.write("""[Desktop Entry] + with open(desktop_path, "w") as f: + f.write( + """[Desktop Entry] Name=Gazebo Client Exec=gzclient Icon=gazebo Type=Application Encoding=UTF-8 Terminal=false - Categories=None;""") + Categories=None;""" + ) os.chmod(desktop_path, 0o755) print("Icono de gzclient creado con éxito en el escritorio.") except Exception as e: