Merge branch 'humble-devel' into testing

Author: Apolo151

LICENSE

@@ -30,17 +30,17 @@ The `Manager` class is the core of RAM, orchestrating operations and managing tr
   - `idle`: The initial state, waiting for a connection.
   - `connected`: Connected and ready to initiate processes.
   - `world_ready`: The world environment is set up and ready.
-  - `visualization_ready`: Visualization tools are prepared and ready.
+  - `tools_ready`: Tools are prepared and ready.
   - `application_running`: A robotic application is actively running.
   - `paused`: The application is paused.
 - **Transitions**:
   - `connect`: Moves from `idle` to `connected`.
   - `launch_world`: Initiates the world setup from `connected`.
-  - `prepare_visualization`: Prepares the visualization tools in `world_ready`.
-  - `run_application`: Starts the application in `visualization_ready` or `paused`.
+  - `prepare_tools`: Prepares the tools in `world_ready`.
+  - `run_application`: Starts the application in `tools_ready` or `paused`.
   - `pause`: Pauses the running application.
   - `resume`: Resumes a paused application.
-  - `terminate`: Stops the application and goes back to `visualization_ready`.
+  - `terminate`: Stops the application and goes back to `tools_ready`.
   - `stop`: Completely stops the application.
   - `disconnect`: Disconnects from the current session and returns to `idle`.
 - **Stateless Transitions**:
@@ -54,7 +54,7 @@ The `Manager` class is the core of RAM, orchestrating operations and managing tr
 
 - `on_connect(self, event)`: Manages the transition to the 'connected' state.
 - `on_launch_world(self, event)`: Prepares and launches the robotic world.
-- `on_prepare_visualization(self, event)`: Sets up visualization tools.
+- `on_prepare_tools(self, event)`: Sets up tools.
 - `on_run_application(self, event)`: Executes the robotic application.
 - `on_pause(self, msg)`: Pauses the running application.
 - `on_resume(self, msg)`: Resumes the paused application.
@@ -84,13 +84,13 @@ The `Manager` class is the core of RAM, orchestrating operations and managing tr
 4. **Termination and Cleanup**: `Manager` can instruct `LauncherWorld` to terminate the world environment through its `terminate` method. `LauncherWorld` ensures a clean and orderly shutdown of all modules and resources involved in the world setup.
 5. **Error Handling and Logging**: `Manager` handles exceptions and errors that may arise during the world setup or termination processes, ensuring robust operation.
 
-#### Interaction Between `Manager` and `LauncherVisualization`
+#### Interaction Between `Manager` and `LauncherTools`
 
-1. **Visualization Setup**: `Manager` initializes `LauncherVisualization` with a specific visualization configuration, which can include types like `console`, `gazebo_gra`, `gazebo_rae`, etc.
-2. **Module Launching for Visualization**: `LauncherVisualization` dynamically launches visualization modules based on the configuration provided by `Manager`.
-3. **State Management and Synchronization**: Upon successful setup of the visualization tools, `Manager` can update its state (e.g., to `visualization_ready`) to reflect the readiness of the visualization environment.
-4. **Termination of Visualization Tools**: `Manager` can instruct `LauncherVisualization` to terminate the current visualization setup using its `terminate` method.
-5. **Error Handling and Logging**: `Manager` is equipped to manage exceptions and errors that might occur during the setup or termination of visualization tools.
+1. **Visualization Setup**: `Manager` initializes `LauncherTools` with a specific tools configuration, which can include tools like `console`, `simulator`, `web_gui`, etc.
+2. **Module Launching for Tools**: `LauncherTools` dynamically launches tools modules based on the configuration provided by `Manager`.
+3. **State Management and Synchronization**: Upon successful setup of the tools, `Manager` can update its state (e.g., to `tools_ready`) to reflect the readiness of the tools.
+4. **Termination of Tools**: `Manager` can instruct `LauncherTools` to terminate the current tools setup using its `terminate` method.
+5. **Error Handling and Logging**: `Manager` is equipped to manage exceptions and errors that might occur during the setup or termination of the tools.
 
 #### Interaction Between `Manager` and `application_process`
 
@@ -100,7 +100,7 @@ The `Manager` class is the core of RAM, orchestrating operations and managing tr
 4. **Error Handling and Logging**: `Manager` is responsible for handling any errors or exceptions that occur during the execution of the `application_process`.
 5. **State Synchronization**: The state of the `application_process` is closely synchronized with the state machine in `Manager`.
 
-#### Interaction Between `Manager` and `Server` (Specific to RoboticsAcademy Applications)
+#### Interaction Between `Manager` and `Server` (Specific to RoboticsAcademy Applications) (Now inside tool web_gui)
 
 1. **Dedicated WebSocket Server for GUI Updates**: `Server` is used exclusively for RoboticsAcademy applications that require real-time interaction with a web-based GUI.
 2. **Client Communication for GUI Module**: For RoboticsAcademy applications with a GUI module, `Server` handles incoming and outgoing messages.
@@ -120,10 +120,10 @@ The `Manager` class is the core of RAM, orchestrating operations and managing tr
    - Once connected, the client can request RAM to launch a robotic world by sending a `launch_world` command.
    - RAM transitions to the `world_ready` state after successfully setting up the world environment.
 
-3. **Setting Up Visualization**:
+3. **Setting Up Tools**:
 
-   - After the world is ready, the client requests RAM to prepare the visualization tools with a `prepare_visualization` command.
-   - RAM transitions to the `visualization_ready` state, indicating that visualization tools are set up and ready.
+   - After the world is ready, the client requests RAM to prepare the tools with a `prepare_tools` command.
+   - RAM transitions to the `tools_ready` state, indicating that the tools are set up and ready.
 
 4. **Running an Application**:
 
@@ -138,7 +138,7 @@ The `Manager` class is the core of RAM, orchestrating operations and managing tr
 6. **Stopping the Application**:
 
    - Finally, the client can send a `stop` command to halt the application.
-   - RAM stops the application and transitions back to the `visualization_ready` state, ready for new commands.
+   - RAM stops the application and transitions back to the `tools_ready` state, ready for new commands.
 
 7. **Disconnecting**:
    - Once all tasks are completed, the client can disconnect from RAM, which then returns to the `idle` state, ready for a new session.

README.md

@@ -8,9 +8,9 @@
 
 
 class ConfigurationModel(BaseModel):
-    """Pydantic model for robotics application world and launch file configuration."""
+    """Pydantic model for robotics application world type and launch file config."""
 
-    world: str
+    type: str
     launch_file_path: str
 
 

manager/libs/launch_world_model.py

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

manager/manager/docker_thread/docker_thread.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/manager/launcher/launcher_console.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/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
@@ -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_gzsim_view.py renamed to manager/manager/launcher/launcher_gzsim.py

@@ -70,7 +70,7 @@
 class LauncherRobot(BaseModel):
     """Class for managing robot launchers in different simulation worlds."""
 
-    world: str
+    type: str
     launch_file_path: str
     module: str = ".".join(__name__.split(".")[:-1])
     ros_version: int = get_ros_version()
@@ -81,7 +81,7 @@ def run(self, start_pose=None):
         """Run the robot launcher with an optional start pose."""
         if start_pose is not 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_robot.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