Merge pull request #283 from JdeRobot/update-robot-api

Add robot reset

Author: javizqh

.github/workflows/python_lint.yaml

@@ -4,31 +4,31 @@ name: Python Linting
 on: [push]
 
 jobs:
-    black-lint:
-        runs-on: ubuntu-latest
-        name: Python black Lint
-        steps:
-            - uses: actions/checkout@v4
-            - uses: psf/black@stable
-              with:
-                version: "22.3.0"
-    # flake8-lint:
-    #     runs-on: ubuntu-latest
-    #     name: Python flake8 Lint
-    #     steps:
-    #         - name: Check out source repository
-    #           uses: actions/checkout@v3
-            
-    #         - name: Set up Python environment
-    #           uses: actions/setup-python@v4
-    #           with:
-    #             python-version: "3.11"
-            
-    #         - name: flake8 Lint
-    #           uses: py-actions/flake8@v2
-    #           with:
-    #             ignore: "E701,W503,E203"
-    #             exclude: "docs,.idea"
-    #             max-line-length: "88"
-    #             path: "."
-    #             plugins: "flake8-docstrings flake8-black"
+  black-lint:
+    runs-on: ubuntu-latest
+    name: Python black Lint
+    steps:
+      - uses: actions/checkout@v4
+      - uses: psf/black@stable
+        with:
+          version: "26.5.1"
+  # flake8-lint:
+  #     runs-on: ubuntu-latest
+  #     name: Python flake8 Lint
+  #     steps:
+  #         - name: Check out source repository
+  #           uses: actions/checkout@v3
+
+  #         - name: Set up Python environment
+  #           uses: actions/setup-python@v4
+  #           with:
+  #             python-version: "3.11"
+
+  #         - name: flake8 Lint
+  #           uses: py-actions/flake8@v2
+  #           with:
+  #             ignore: "E701,W503,E203"
+  #             exclude: "docs,.idea"
+  #             max-line-length: "88"
+  #             path: "."
+  #             plugins: "flake8-docstrings flake8-black"

robotics_application_manager/manager/launcher/launcher_console.py

@@ -45,7 +45,7 @@ def pause(self):
     def unpause(self):
         pass
 
-    def reset(self):
+    def reset(self, robot_entity=None):
         pass
 
     def is_running(self):

robotics_application_manager/manager/launcher/launcher_gazebo.py

@@ -1,9 +1,9 @@
 """
 Gazebo Classic Launcher and Lifecycle Manager.
 
-Handles the initialization, state management, and cleanup of the 
-Gazebo GUI client (gzclient). Orchestrates display routing via 
-VNC and provides hardware-accelerated rendering through VirtualGL 
+Handles the initialization, state management, and cleanup of the
+Gazebo GUI client (gzclient). Orchestrates display routing via
+VNC and provides hardware-accelerated rendering through VirtualGL
 when compatible DRI devices are detected.
 """
 
@@ -106,7 +106,7 @@ def unpause(self):
         """Resumes the physics engine via the /unpause_physics ROS 2 service."""
         call_service("/unpause_physics", "std_srvs/srv/Empty")
 
-    def reset(self):
+    def reset(self, robot_entity=None):
         call_service("/reset_world", "std_srvs/srv/Empty")
 
     def is_running(self):

robotics_application_manager/manager/launcher/launcher_gzsim.py

@@ -13,6 +13,12 @@
 from typing import List, Any
 from robotics_application_manager import LogManager
 
+from gz.msgs10.world_control_pb2 import WorldControl
+from gz.msgs10.world_reset_pb2 import WorldReset
+from gz.msgs10.entity_pb2 import Entity
+from gz.msgs10.boolean_pb2 import Boolean
+from gz.transport13 import Node
+
 
 def call_gzservice(service, reqtype, reptype, timeout, req):
     command = f"gz service -s {service} --reqtype {reqtype} --reptype {reptype} --timeout {timeout} --req '{req}'"
@@ -150,20 +156,32 @@ def unpause(self):
             "pause: false",
         )
 
-    def reset(self):
+    def reset(self, robot_entity=None):
         if is_ros_service_available("/drone0/platform/state_machine/_reset"):
             call_service(
                 "/drone0/platform/state_machine/_reset",
                 "std_srvs/srv/Trigger",
                 "{}",
             )
-        call_gzservice(
-            "/world/default/control",
-            "gz.msgs.WorldControl",
-            "gz.msgs.Boolean",
-            "3000",
-            "reset: {all: true}",
+        node = Node()
+
+        if robot_entity is not None:
+            node.request(
+                f"/world/default/remove",
+                Entity(name=robot_entity, type=Entity.MODEL),
+                Entity,
+                Boolean,
+                1000,
+            )
+
+        node.request(
+            f"/world/default/control",
+            WorldControl(pause=True, reset=WorldReset(all=True)),
+            WorldControl,
+            Boolean,
+            1000,
         )
+
         if is_ros_service_available("/drone0/controller/_reset"):
             call_service("/drone0/controller/_reset", "std_srvs/srv/Trigger", "{}")
 

robotics_application_manager/manager/launcher/launcher_o3de.py

@@ -50,7 +50,7 @@ def unpause(self):
         self.running = True
         pass
 
-    def reset(self):
+    def reset(self, robot_entity=None):
         # TODO: add reset
         pass
 

robotics_application_manager/manager/launcher/launcher_robot.py

@@ -13,55 +13,20 @@
 
 worlds = {
     "gazebo": {
-        "1": [
-            {
-                "type": "module",
-                "module": "ros_api",
-                "parameters": [],
-                "launch_file": [],
-            }
-        ],
         "2": [
             {
-                "type": "module",
+                "type": "gazebo",
                 "module": "robot_ros2_api",
                 "parameters": [],
                 "launch_file": [],
             }
         ],
     },
-    "drones": {
-        "1": [
-            {
-                "type": "module",
-                "module": "drones",
-                "resource_folders": [],
-                "model_folders": [],
-                "plugin_folders": [],
-                "parameters": [],
-                "launch_file": [],
-            }
-        ],
-        "2": [
-            {
-                "type": "module",
-                "module": "drones_ros2",
-                "resource_folders": [],
-                "model_folders": [],
-                "plugin_folders": [],
-                "parameters": [],
-                "launch_file": [],
-            }
-        ],
-    },
-    "gzsimdrones": {
+    "gz": {
         "2": [
             {
-                "type": "module",
-                "module": "drones_gzsim",
-                "resource_folders": [],
-                "model_folders": [],
-                "plugin_folders": [],
+                "type": "gz",
+                "module": "robot_ros2_api",
                 "parameters": [],
                 "launch_file": [],
             }
@@ -81,13 +46,17 @@ class LauncherRobot(BaseModel):
     launchers: Optional[ILauncher] = []
     start_pose: Optional[list] = []
 
-    def run(self, start_pose=None):
+    def run(self, start_pose=None, extra_config=None):
         """Run the robot launcher with an optional start pose."""
         if start_pose is not None:
             self.start_pose = start_pose
+
+        if extra_config is None:
+            extra_config = ""
+
         for module in worlds[self.type][str(self.ros_version)]:
             module["launch_file"] = self.launch_file_path
-            launcher = self.launch_module(module)
+            launcher = self.launch_module(module, extra_config)
             self.launchers.append(launcher)
         LogManager.logger.info(self.launchers)
 
@@ -99,7 +68,7 @@ def terminate(self):
                 launcher.terminate()
         self.launchers = []
 
-    def launch_module(self, configuration):
+    def launch_module(self, configuration, extra_config=None):
         """Launch a robot module based on the provided configuration."""
 
         def process_terminated(name, exit_code):
@@ -117,7 +86,7 @@ def process_terminated(name, exit_code):
         launcher_class = get_class(launcher_module)
         launcher = launcher_class.from_config(launcher_class, configuration)
 
-        launcher.run(self.start_pose, process_terminated)
+        launcher.run(self.start_pose, extra_config, process_terminated)
         return launcher
 
     def launch_command(self, configuration):

robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py

@@ -9,8 +9,31 @@
 )
 from robotics_application_manager.manager.docker_thread import DockerThread
 import subprocess
+import sys
 
 import logging
+from robotics_application_manager import LogManager
+
+
+def call_service(service, service_type, request_data="{}"):
+    command = f"ros2 service call {service} {service_type} '{request_data}'"
+    try:
+        p = subprocess.Popen(
+            [
+                f"{command}",
+            ],
+            shell=True,
+            stdout=sys.stdout,
+            stderr=subprocess.STDOUT,
+            bufsize=1024,
+            universal_newlines=True,
+        )
+        p.wait(10)
+    except:
+        p.kill()
+
+        LogManager.logger.exception(f"Unable to complete call: {service}")
+        raise Exception(f"Unable to complete call: {service}")
 
 
 class LauncherRobotRos2Api(ILauncher):
@@ -19,7 +42,7 @@ class LauncherRobotRos2Api(ILauncher):
     launch_file: str
     threads: List[Any] = []
 
-    def run(self, robot_pose, callback):
+    def run(self, robot_pose, extra_config, callback):
         DRI_PATH = self.get_dri_path()
         ACCELERATION_ENABLED = self.check_device(DRI_PATH)
 
@@ -30,12 +53,15 @@ def run(self, robot_pose, callback):
         xserver_thread.start()
         self.threads.append(xserver_thread)
 
-        ROBOT_POSE = f"ROBOT_X={robot_pose[0]} ROBOT_Y={robot_pose[1]} ROBOT_Z={robot_pose[2]} ROBOT_ROLL={robot_pose[3]} ROBOT_PITCH={robot_pose[4]} ROBOT_YAW={robot_pose[5]}"
+        x, y, z, R, P, Y = robot_pose
+
+        if extra_config == "None":
+            extra_config = ""
 
         if ACCELERATION_ENABLED:
-            exercise_launch_cmd = f"export VGL_DISPLAY={DRI_PATH}; vglrun {ROBOT_POSE} ros2 launch {self.launch_file}"
+            exercise_launch_cmd = f"export VGL_DISPLAY={DRI_PATH}; vglrun ros2 launch {self.launch_file} x:={x} y:={y} z:={z} R:={R} P:={P} Y:={Y} {extra_config}"
         else:
-            exercise_launch_cmd = f"{ROBOT_POSE} ros2 launch {self.launch_file}"
+            exercise_launch_cmd = f"ros2 launch {self.launch_file} x:={x} y:={y} z:={z} R:={R} P:={P} Y:={Y} {extra_config}"
 
         exercise_launch_thread = DockerThread(exercise_launch_cmd)
         exercise_launch_thread.start()
@@ -58,8 +84,7 @@ def terminate(self):
             universal_newlines=True,
         )
 
-        kill_cmd = "pkill -9 "
-        cmd = kill_cmd + "bridg"
+        cmd = kill_cmd + "bridge"
         subprocess.call(
             cmd,
             shell=True,

robotics_application_manager/manager/launcher/launcher_rviz.py

@@ -68,7 +68,7 @@ def pause(self):
     def unpause(self):
         pass
 
-    def reset(self):
+    def reset(self, robot_entity=None):
         pass
 
     def is_running(self):

robotics_application_manager/manager/launcher/launcher_state_monitor.py

@@ -40,7 +40,7 @@ def pause(self):
     def unpause(self):
         pass
 
-    def reset(self):
+    def reset(self, robot_entity=None):
         pass
 
     def died(self):

robotics_application_manager/manager/launcher/launcher_tools.py

@@ -131,9 +131,9 @@ def unpause(self):
         for launcher in self.launchers:
             launcher.unpause()
 
-    def reset(self):
+    def reset(self, robot_entity=None):
         for launcher in self.launchers:
-            launcher.reset()
+            launcher.reset(robot_entity)
 
     def pass_msg(self, data):
         for launcher in self.launchers: