Merge pull request #18 from JdeRobot/issue-17

ReyDoran · web-flow · commit 2161605a6edd · 2023-05-16T17:47:09.000+02:00
Added ROS2 support
diff --git a/manager/libs/applications/compatibility/exercise_wrapper_ros2.py b/manager/libs/applications/compatibility/exercise_wrapper_ros2.py
@@ -0,0 +1,162 @@
+import json
+import logging
+import os.path
+import subprocess
+import sys
+import threading
+import time
+from threading import Thread
+
+from src.manager.libs.applications.compatibility.client import Client
+from src.manager.libs.process_utils import stop_process_and_children
+from src.manager.ram_logging.log_manager import LogManager
+from src.manager.manager.application.robotics_python_application_interface import IRoboticsPythonApplication
+from src.manager.manager.lint.linter import Lint
+
+
+class CompatibilityExerciseWrapperRos2(IRoboticsPythonApplication):
+    def __init__(self, exercise_command, gui_command, update_callback):
+        super().__init__(update_callback)
+
+        home_dir = os.path.expanduser('~')
+        self.running = False
+        self.linter = Lint()
+        self.brain_ready_event = threading.Event()
+        # TODO: review hardcoded values
+        process_ready, self.exercise_server = self._run_exercise_server(f"python3 {exercise_command}",
+                                                                        f'{home_dir}/ws_code.log',
+                                                                        'websocket_code=ready')
+        if process_ready:
+            LogManager.logger.info(
+                f"Exercise code {exercise_command} launched")
+            time.sleep(1)
+            self.exercise_connection = Client(
+                'ws://127.0.0.1:1905', 'exercise', self.server_message)
+            self.exercise_connection.start()
+        else:
+            self.exercise_server.kill()
+            raise RuntimeError(f"Exercise {exercise_command} could not be run")
+
+        process_ready, self.gui_server = self._run_exercise_server(f"python3 {gui_command}", f'{home_dir}/ws_gui.log',
+                                                                   'websocket_gui=ready')
+        if process_ready:
+            LogManager.logger.info(f"Exercise gui {gui_command} launched")
+            time.sleep(1)
+            self.gui_connection = Client(
+                'ws://127.0.0.1:2303', 'gui', self.server_message)
+            self.gui_connection.start()
+        else:
+            self.gui_server.kill()
+            raise RuntimeError(f"Exercise GUI {gui_command} could not be run")
+        
+        self.running = True
+
+        self.start_send_freq_thread()
+
+
+    def send_freq(self, exercise_connection, is_alive):
+        """Send the frequency of the brain and gui to the exercise server"""
+        while is_alive():
+            exercise_connection.send(
+                """#freq{"brain": 20, "gui": 10, "rtf": 100}""")
+            time.sleep(1)
+
+    def start_send_freq_thread(self):
+        """Start a thread to send the frequency of the brain and gui to the exercise server"""
+        daemon = Thread(target=lambda: self.send_freq(self.exercise_connection,
+                        lambda: self.is_alive), daemon=False, name='Monitor frequencies')
+        daemon.start()
+
+    def _run_exercise_server(self, cmd, log_file, load_string, timeout: int = 5):
+        process = subprocess.Popen(f"{cmd}", shell=True, stdout=sys.stdout, stderr=subprocess.STDOUT,
+                                   bufsize=1024, universal_newlines=True)
+
+        process_ready = False
+        while not process_ready:
+            try:
+                f = open(log_file, "r")
+                if f.readline() == load_string:
+                    process_ready = True
+                f.close()
+                time.sleep(0.2)
+            except Exception as e:
+                LogManager.logger.debug(
+                    f"waiting for server string '{load_string}'...")
+                time.sleep(0.2)
+
+        return process_ready, process
+
+    def server_message(self, name, message):
+        if name == "gui":  # message received from GUI server
+            LogManager.logger.debug(
+                f"Message received from gui: {message[:30]}")
+            self._process_gui_message(message)
+        elif name == "exercise":  # message received from EXERCISE server
+            if message.startswith("#exec"):
+                self.brain_ready_event.set()
+            LogManager.logger.info(
+                f"Message received from exercise: {message[:30]}")
+            self._process_exercise_message(message)
+
+    def _process_gui_message(self, message):
+        payload = json.loads(message[4:])
+        self.update_callback(payload)
+        self.gui_connection.send("#ack")
+
+    def _process_exercise_message(self, message):
+        comand = message[:5]
+        if (message==comand):
+            payload = comand
+        else:
+            payload = json.loads(message[5:])
+        self.update_callback(payload)
+        self.exercise_connection.send("#ack")
+
+    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)
+    
+    def run(self):
+        self.call_service("/unpause_physics","std_srvs/srv/Empty")
+        self.exercise_connection.send("#play")
+
+    def stop(self):
+        self.call_service("/pause_physics","std_srvs/srv/Empty")
+        self.call_service("/reset_world","std_srvs/srv/Empty")
+        self.exercise_connection.send("#rest")
+
+    def resume(self):
+        self.call_service("/unpause_physics","std_srvs/srv/Empty")
+        self.exercise_connection.send("#play")
+
+    def pause(self):
+        self.call_service("/pause_physics","std_srvs/srv/Empty")
+        self.exercise_connection.send("#stop")
+
+    def restart(self):
+        # pause_cmd = "ros2 service call /restart_simulation std_srvs/srv/Empty"
+        # subprocess.call(f"{pause_cmd}", shell=True, stdout=sys.stdout, stderr=subprocess.STDOUT, bufsize=1024,
+        #                            universal_newlines=True)
+        pass
+
+    @property
+    def is_alive(self):
+        return self.running
+
+    def load_code(self, code: str):
+        errors = self.linter.evaluate_code(code)
+        if errors == "":
+            self.brain_ready_event.clear()
+            self.exercise_connection.send(f"#code {code}")
+            self.brain_ready_event.wait()
+        else:
+            raise Exception(errors)
+
+    def terminate(self):
+        self.running = False
+        self.exercise_connection.stop()
+        self.gui_connection.stop()
+
+        stop_process_and_children(self.exercise_server)
+        stop_process_and_children(self.gui_server)
diff --git a/manager/manager/launcher/launcher_console_ros2.py b/manager/manager/launcher/launcher_console_ros2.py
@@ -0,0 +1,53 @@
+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
+import time
+import os
+import stat
+
+
+class LauncherConsoleRos2(ILauncher):
+    display: str
+    internal_port: str
+    external_port: str
+    running = False
+    threads = []
+
+    def run(self, callback):
+        DRI_PATH = os.path.join("/dev/dri", os.environ.get("DRI_NAME", "card0"))
+        ACCELERATION_ENABLED = self.check_device(DRI_PATH)
+
+        console_vnc = Vnc_server()
+        
+        if (ACCELERATION_ENABLED):
+            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:
+            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"
+
+        console_thread = DockerThread(console_cmd)
+        console_thread.start()
+        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])
+        except:
+            return False
+
+    def is_running(self):
+        return self.running
+
+    def terminate(self):
+        for thread in self.threads:
+            thread.terminate()
+            thread.join()
+        self.running = False
+
+    def died(self):
+        pass
diff --git a/manager/manager/launcher/launcher_gazebo_view_ros2.py b/manager/manager/launcher/launcher_gazebo_view_ros2.py
@@ -0,0 +1,67 @@
+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
+import time
+import os
+import stat
+
+
+class LauncherGazeboViewRos2(ILauncher):
+    exercise_id: str
+    display: str
+    internal_port: str
+    external_port: str
+    height: int
+    width: int
+    running = False
+    threads = []
+
+    def run(self, callback):
+        DRI_PATH = os.path.join(
+            "/dev/dri", os.environ.get("DRI_NAME", "card0"))
+        ACCELERATION_ENABLED = self.check_device(DRI_PATH)
+
+        # Configure browser screen width and height for gzclient
+        gzclient_config_cmds = f"echo [geometry] > ~/.gazebo/gui.ini; echo x=0 >> ~/.gazebo/gui.ini; echo y=0 >> ~/.gazebo/gui.ini; echo width={self.width} >> ~/.gazebo/gui.ini; echo height={self.height} >> ~/.gazebo/gui.ini;"
+        gz_vnc = Vnc_server()
+
+        if ACCELERATION_ENABLED:
+            gz_vnc.start_vnc_gpu(self.display, self.internal_port, self.external_port, DRI_PATH)
+            # Write display config and start gzclient
+            gzclient_cmd = (
+                f"export DISPLAY=:0; {gzclient_config_cmds} export VGL_DISPLAY={DRI_PATH}; vglrun gzclient --verbose")
+        else:
+            gz_vnc.start_vnc(self.display, self.internal_port, self.external_port)
+            # Write display config and start gzclient
+            gzclient_cmd = (
+                f"export DISPLAY=:0; {gzclient_config_cmds} gzclient --verbose")
+
+        # wait for vnc and gazebo servers to load properly
+        if (self.exercise_id == "follow_person_newmanager"):
+            time.sleep(6)
+        else:
+            time.sleep(0.1)
+
+        gzclient_thread = DockerThread(gzclient_cmd)
+        gzclient_thread.start()
+        self.threads.append(gzclient_thread)
+
+        self.running = True
+
+    def check_device(self, device_path):
+        try:
+            return stat.S_ISCHR(os.lstat(device_path)[stat.ST_MODE])
+        except:
+            return False
+
+    def is_running(self):
+        return self.running
+
+    def terminate(self):
+        for thread in self.threads:
+            thread.terminate()
+            thread.join()
+        self.running = False
+
+    def died(self):
+        pass
diff --git a/manager/manager/launcher/launcher_ros2_api.py b/manager/manager/launcher/launcher_ros2_api.py
@@ -0,0 +1,67 @@
+import os
+from typing import List, Any
+import time
+import stat
+
+from src.manager.manager.launcher.launcher_interface import ILauncher, LauncherException
+from src.manager.manager.docker_thread.docker_thread import DockerThread
+import subprocess
+
+import logging
+
+class LauncherRos2Api(ILauncher):
+    exercise_id: str
+    type: str
+    module: str
+    resource_folders: List[str]
+    model_folders: List[str]
+    plugin_folders: List[str]
+    parameters: List[str]
+    launch_file: str
+    running = False
+
+    def run(self,callback):
+        DRI_PATH = os.path.join("/dev/dri", os.environ.get("DRI_NAME", "card0"))
+        ACCELERATION_ENABLED = self.check_device(DRI_PATH)
+
+        logging.getLogger("roslaunch").setLevel(logging.CRITICAL)
+
+        # expand variables in configuration paths
+        self._set_environment()
+        launch_file = os.path.expandvars(self.launch_file)
+
+        if (ACCELERATION_ENABLED):
+            exercise_launch_cmd = f"export VGL_DISPLAY={DRI_PATH}; vglrun ros2 launch {launch_file}"
+        else:
+            exercise_launch_cmd = f"ros2 launch {launch_file}"
+
+        exercise_launch_thread = DockerThread(exercise_launch_cmd)
+        exercise_launch_thread.start()
+
+        self.running = True
+
+    def is_running(self):
+        return self.running
+    
+    def check_device(self, device_path):
+        try:
+            return stat.S_ISCHR(os.lstat(device_path)[stat.ST_MODE])
+        except:
+            return False
+
+    def terminate(self):
+        if self.is_running():
+            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)
+
+    def _set_environment(self):
+        resource_folders = [os.path.expandvars(path) for path in self.resource_folders]
+        model_folders = [os.path.expandvars(path) for path in self.model_folders]
+        plugin_folders = [os.path.expandvars(path) for path in self.plugin_folders]
+
+        os.environ["GAZEBO_RESOURCE_PATH"] = f"{os.environ.get('GAZEBO_RESOURCE_PATH', '')}:{':'.join(resource_folders)}"
+        os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(model_folders)}"
+        os.environ["GAZEBO_PLUGIN_PATH"] = f"{os.environ.get('GAZEBO_PLUGIN_PATH', '')}:{':'.join(plugin_folders)}"
diff --git a/manager/manager/lint/linter.py b/manager/manager/lint/linter.py
@@ -27,7 +27,14 @@ def evaluate_code(self, code, warnings=False):
 
             open("user_code.py", "r")
 
-            command = "export PYTHONPATH=$PYTHONPATH:/$EXERCISE_FOLDER/python_template/ros1_noetic; python3 RoboticsAcademy/src/manager/manager/lint/pylint_checker.py"
+            command = ""
+            output = subprocess.check_output(['bash', '-c', 'echo $ROS_VERSION'])
+            output_str = output.decode('utf-8')
+            version = output_str[0]
+            if (version == 2):                
+                command = "export PYTHONPATH=$PYTHONPATH:/$EXERCISE_FOLDER/python_template/ros2_humble; python3 RoboticsAcademy/src/manager/manager/lint/pylint_checker.py"
+            else:
+                command = "export PYTHONPATH=$PYTHONPATH:/$EXERCISE_FOLDER/python_template/ros1_noetic; python3 RoboticsAcademy/src/manager/manager/lint/pylint_checker.py"
             ret = subprocess.run(command, capture_output=True, shell=True)
             result = ret.stdout.decode()
             result = result + "\n"
diff --git a/manager/manager/lint/pylintrc b/manager/manager/lint/pylintrc
diff --git a/manager/manager/vnc/vnc_server.py b/manager/manager/vnc/vnc_server.py
