Adding neutral tools

Author: javizqh

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
@@ -11,14 +12,25 @@
 import stat
 from typing import List, Any
 
+def call_service(self, 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 LauncherGazeboView(ILauncher):
+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 +63,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,44 @@
 import os
 import stat
 from typing import List, Any
+from manager.ram_logging.log_manager import LogManager
 
+def call_gzservice(self, 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,
+    )
 
-class LauncherGzsimView(ILauncher):
+def call_service(self, 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(self, 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 +109,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_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