Merge pull request #223 from JdeRobot/neutral-steps

Change visualization to tools

Author: javizqh

manager/libs/launch_world_model.py

@@ -6,7 +6,7 @@
 
 
 class ConfigurationModel(BaseModel):
-    world: str
+    type: str
     launch_file_path: str
 
 

manager/manager/docker_thread/docker_thread.py

@@ -4,6 +4,7 @@
 import subprocess
 import os
 import signal
+import sys
 
 
 class DockerThread(threading.Thread):

manager/manager/launcher/launcher_console.py

@@ -12,6 +12,7 @@ class LauncherConsole(ILauncher):
     internal_port: int
     external_port: int
     running: bool = False
+    acceptsMsgs: bool = False
     threads: List[Any] = []
     console_vnc: Any = Vnc_server()
 
@@ -38,6 +39,15 @@ def run(self, config_file, callback):
 
         self.running = True
 
+    def pause(self):
+        pass
+
+    def unpause(self):
+        pass
+
+    def reset(self):
+        pass
+
     def is_running(self):
         return self.running
 

…manager/launcher/launcher_gazebo_view.py …ager/manager/launcher/launcher_gazebo.pymanager/manager/launcher/launcher_gazebo_view.py renamed to manager/manager/launcher/launcher_gazebo.py manager/manager/launcher/launcher_gazebo_view.py renamed to manager/manager/launcher/launcher_gazebo.py

@@ -1,3 +1,4 @@
+import sys
 from manager.manager.launcher.launcher_interface import ILauncher
 from manager.manager.docker_thread.docker_thread import DockerThread
 from manager.manager.vnc.vnc_server import Vnc_server
@@ -12,13 +13,26 @@
 from typing import List, Any
 
 
-class LauncherGazeboView(ILauncher):
+def call_service(service, service_type, request_data="{}"):
+    command = f"ros2 service call {service} {service_type} '{request_data}'"
+    subprocess.call(
+        f"{command}",
+        shell=True,
+        stdout=sys.stdout,
+        stderr=subprocess.STDOUT,
+        bufsize=1024,
+        universal_newlines=True,
+    )
+
+
+class LauncherGazebo(ILauncher):
     display: str
     internal_port: int
     external_port: int
     height: int
     width: int
     running: bool = False
+    acceptsMsgs: bool = False
     threads: List[Any] = []
     gz_vnc: Any = Vnc_server()
 
@@ -51,6 +65,15 @@ def run(self, config_file, callback):
 
         self.running = True
 
+    def pause(self):
+        call_service("/pause_physics", "std_srvs/srv/Empty")
+
+    def unpause(self):
+        call_service("/unpause_physics", "std_srvs/srv/Empty")
+
+    def reset(self):
+        call_service("/reset_world", "std_srvs/srv/Empty")
+
     def is_running(self):
         return self.running
 

…/manager/launcher/launcher_gzsim_view.py …nager/manager/launcher/launcher_gzsim.pymanager/manager/launcher/launcher_gzsim_view.py renamed to manager/manager/launcher/launcher_gzsim.py manager/manager/launcher/launcher_gzsim_view.py renamed to manager/manager/launcher/launcher_gzsim.py

@@ -1,3 +1,4 @@
+import sys
 from manager.manager.launcher.launcher_interface import ILauncher
 from manager.manager.docker_thread.docker_thread import DockerThread
 from manager.manager.vnc.vnc_server import Vnc_server
@@ -10,9 +11,48 @@
 import os
 import stat
 from typing import List, Any
+from manager.ram_logging.log_manager import LogManager
 
 
-class LauncherGzsimView(ILauncher):
+def call_gzservice(service, reqtype, reptype, timeout, req):
+    command = f"gz service -s {service} --reqtype {reqtype} --reptype {reptype} --timeout {timeout} --req '{req}'"
+    subprocess.call(
+        f"{command}",
+        shell=True,
+        stdout=sys.stdout,
+        stderr=subprocess.STDOUT,
+        bufsize=1024,
+        universal_newlines=True,
+    )
+
+
+def call_service(service, service_type, request_data="{}"):
+    command = f"ros2 service call {service} {service_type} '{request_data}'"
+    subprocess.call(
+        f"{command}",
+        shell=True,
+        stdout=sys.stdout,
+        stderr=subprocess.STDOUT,
+        bufsize=1024,
+        universal_newlines=True,
+    )
+
+
+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
     external_port: int
@@ -73,6 +113,41 @@ def terminate(self):
     def died(self):
         pass
 
+    def pause(self):
+        call_gzservice(
+            "$(gz service -l | grep '^/world/\w*/control$')",
+            "gz.msgs.WorldControl",
+            "gz.msgs.Boolean",
+            "3000",
+            "pause: true",
+        )
+
+    def unpause(self):
+        call_gzservice(
+            "$(gz service -l | grep '^/world/\w*/control$')",
+            "gz.msgs.WorldControl",
+            "gz.msgs.Boolean",
+            "3000",
+            "pause: false",
+        )
+
+    def reset(self):
+        if is_ros_service_available("/drone0/platform/state_machine/_reset"):
+            call_service(
+                "/drone0/platform/state_machine/_reset",
+                "std_srvs/srv/Trigger",
+                "{}",
+            )
+        call_gzservice(
+            "$(gz service -l | grep '^/world/\w*/control$')",
+            "gz.msgs.WorldControl",
+            "gz.msgs.Boolean",
+            "3000",
+            "reset: {all: true}",
+        )
+        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 = ""

manager/manager/launcher/launcher_robot.py

@@ -66,7 +66,7 @@
 
 
 class LauncherRobot(BaseModel):
-    world: str
+    type: str
     launch_file_path: str
     module: str = ".".join(__name__.split(".")[:-1])
     ros_version: int = get_ros_version()
@@ -76,7 +76,7 @@ class LauncherRobot(BaseModel):
     def run(self, start_pose=None):
         if start_pose != None:
             self.start_pose = start_pose
-        for module in worlds[self.world][str(self.ros_version)]:
+        for module in worlds[self.type][str(self.ros_version)]:
             module["launch_file"] = self.launch_file_path
             launcher = self.launch_module(module)
             self.launchers.append(launcher)

manager/manager/launcher/launcher_state_monitor.py

@@ -0,0 +1,52 @@
+from manager.libs.applications.compatibility.server import Server
+from manager.ram_logging.log_manager import LogManager
+from manager.comms.new_consumer import ManagerConsumer
+from typing import Optional
+from manager.libs.applications.compatibility.file_watchdog import FileWatchdog
+
+
+class LauncherStateMonitor:
+    file: str
+    consumer: ManagerConsumer
+    running: bool = False
+    acceptsMsgs: bool = True
+
+    def __init__(self, type, module, file, consumer):
+        self.file = file
+        self.consumer = consumer
+        self.server = FileWatchdog("/tmp/tree_state", self.update)
+
+    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")
+
+    def run(self, config_file, callback):
+        self.server.start()
+        self.running = True
+
+    def get_msg(self, data):
+        self.server.send(data)
+
+    def is_running(self):
+        return self.running
+
+    def terminate(self):
+        self.server.stop()
+        self.running = False
+
+    def pause(self):
+        pass
+
+    def unpause(self):
+        pass
+
+    def reset(self):
+        pass
+
+    def died(self):
+        pass
+
+    def from_config(cls, config):
+        obj = cls(**config)
+        return obj

manager/manager/launcher/launcher_tools.py

@@ -0,0 +1,123 @@
+from manager.libs.process_utils import get_class, class_from_module
+from typing import Optional
+from pydantic import BaseModel
+
+
+from manager.libs.process_utils import get_class, class_from_module
+from manager.ram_logging.log_manager import LogManager
+from manager.manager.launcher.launcher_interface import ILauncher
+
+tools = {
+    "console": {
+        "module": "console",
+        "display": ":1",
+        "external_port": 1108,
+        "internal_port": 5901,
+    },
+    "gazebo": {
+        "type": "module",
+        "width": 1024,
+        "height": 768,
+        "module": "gazebo",
+        "display": ":2",
+        "external_port": 6080,
+        "internal_port": 5900,
+    },
+    "gzsim": {
+        "type": "module",
+        "width": 1024,
+        "height": 768,
+        "module": "gzsim",
+        "display": ":2",
+        "external_port": 6080,
+        "internal_port": 5900,
+    },
+    "web_gui": {
+        "type": "module",
+        "module": "web_gui",
+        "internal_port": 2303,
+        "consumer": None,
+    },
+    "state_monitor": {
+        "type": "module",
+        "module": "state_monitor",
+        "file": "/tmp/tree_state",
+        "consumer": None,
+    },
+}
+
+simulator = {
+    "gazebo": {"tool": "gazebo"},
+    "gz": {"tool": "gzsim"},
+}
+
+
+class LauncherTools(BaseModel):
+    module: str = ".".join(__name__.split(".")[:-1])
+    world_type: Optional[str] = None
+    tools: list[str]
+    tools_config: Optional[dict] = None
+    launchers: Optional[ILauncher] = []
+
+    def run(self, consumer):
+        for tool in self.tools:
+            if tool == "simulator":
+                tool = simulator[self.world_type]["tool"]
+            module = tools[tool]
+            launcher = self.launch_module(tool, module, consumer)
+            self.launchers.append(launcher)
+
+    def terminate(self):
+        LogManager.logger.info("Terminating tools launcher")
+        for launcher in self.launchers:
+            if launcher.is_running():
+                launcher.terminate()
+        self.launchers = []
+
+    def launch_module(self, name, configuration, consumer):
+        def process_terminated(name, exit_code):
+            LogManager.logger.info(
+                f"LauncherEngine: {name} exited with code {exit_code}"
+            )
+            if self.terminated_callback is not None:
+                self.terminated_callback(name, exit_code)
+
+        # Replace consumer
+        if "consumer" in configuration:
+            configuration["consumer"] = consumer
+
+        launcher_module_name = configuration["module"]
+        launcher_module = f"{self.module}.launcher_{launcher_module_name}.Launcher{class_from_module(launcher_module_name)}"
+        launcher_class = get_class(launcher_module)
+        config = None
+        if self.tools_config is not None and name in self.tools_config:
+            config = self.tools_config[name]
+
+        launcher = launcher_class.from_config(launcher_class, configuration)
+        launcher.run(config, process_terminated)
+        return launcher
+
+    def pause(self):
+        for launcher in self.launchers:
+            launcher.pause()
+
+    def unpause(self):
+        for launcher in self.launchers:
+            launcher.unpause()
+
+    def reset(self):
+        for launcher in self.launchers:
+            launcher.reset()
+
+    def pass_msg(self, data):
+        for launcher in self.launchers:
+            if launcher.acceptsMsgs:
+                launcher.get_msg(data)
+
+    def launch_command(self, configuration):
+        pass
+
+
+class LauncherToolsException(Exception):
+    def __init__(self, message):
+        super(LauncherToolsException, self).__init__(message)