Change visualization to tools #223

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged

javizqh merged 16 commits into humble-devel from neutral-steps

Jul 15, 2025

manager/libs/launch_world_model.py

-Original file line number
+Diff line change
@@ Expand Up / @@ -6,7 +6,7 @@ @@
     class ConfigurationModel(BaseModel):
-        world: str
+        type: str
         launch_file_path: str
@@ Expand Down @@

manager/manager/docker_thread/docker_thread.py

-Original file line number
+Diff line change
@@ Expand Up / @@ -4,6 +4,7 @@ @@
     import subprocess
     import os
     import signal
+    import sys
     class DockerThread(threading.Thread):
@@ Expand Down @@

manager/manager/launcher/launcher_console.py

-Original file line number
+Diff line change
@@ Expand Up / @@ -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()
@@ Expand All / @@ -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
@@ Expand Down @@

.../manager/launcher/launcher_gazebo_view.py → manager/manager/launcher/launcher_gazebo.py

-Original file line number
+Diff line change
@@ -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
@@ Expand All / @@ -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()
@@ Expand Down Expand Up / @@ -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
@@ Expand Down @@

...r/manager/launcher/launcher_gzsim_view.py → manager/manager/launcher/launcher_gzsim.py

-Original file line number
+Diff line change
@@ -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
@@ Expand All / @@ -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
@@ Expand Down Expand Up / @@ -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 = ""
@@ Expand Down @@

manager/manager/launcher/launcher_robot.py

            
                      Original file line number
                      Diff line number
                      Diff line change
                  
    @@ -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

-Original file line number
+Diff line change
@@ -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

-Original file line number
+Diff line change
@@ -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)

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Change visualization to tools #223

Uh oh!

Diff view

Diff view

There are no files selected for viewing

Uh oh!