Add robot reset

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:

robotics_application_manager/manager/launcher/launcher_web_gui.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/manager.py

@@ -225,6 +225,7 @@ def __init__(self, host: str, port: int):
         self.world_launcher = None
         self.world_type = None
         self.robot_launcher = None
+        self.robot_config = None
         self.tools_launcher = None
         self.application_process = None
         self.running = True
@@ -345,25 +346,24 @@ def on_launch_world(self, event):
 
         self.world_launcher = LauncherWorld(**cfg.model_dump())
         LogManager.logger.info(str(self.world_launcher))
-        self.world_launcher.run()
-        LogManager.logger.info("Launch transition finished")
 
         # Launch robot
-        try:
-            if robot_cfg["type"] == None:
-                self.robot_launcher = None
-                LogManager.logger.info("Launch transition finished")
-                return
-            cfg = ConfigurationManager.validate(robot_cfg)
-            LogManager.logger.info("Launching robot from the RB")
+        self.robot_launcher = None
+        if robot_cfg["type"] is not None:
+            try:
+                cfg = ConfigurationManager.validate(robot_cfg)
+                LogManager.logger.info("Launching robot from the RB")
+                LogManager.logger.info(cfg)
+            except ValueError as e:
+                LogManager.logger.error(f"Configuration validation failed: {e}")
 
-            LogManager.logger.info(cfg)
-        except ValueError as e:
-            LogManager.logger.error(f"Configuration validation failed: {e}")
+            self.robot_launcher = LauncherRobot(**cfg.model_dump())
+            self.robot_config = robot_cfg
+            LogManager.logger.info(str(self.robot_launcher))
 
-        self.robot_launcher = LauncherRobot(**cfg.model_dump())
-        LogManager.logger.info(str(self.robot_launcher))
-        self.robot_launcher.run(robot_cfg["start_pose"])
+        self.world_launcher.run()
+        if self.robot_launcher is not None:
+            self.robot_launcher.run(robot_cfg["start_pose"], robot_cfg["extra_config"])
         LogManager.logger.info("Launch transition finished")
 
     def prepare_custom_universe(self, cfg_dict):
@@ -995,14 +995,19 @@ def reset_sim(self):
             self.robot_launcher.terminate()
 
         try:
-            self.tools_launcher.reset()
+            entity = None
+            if self.robot_config is not None:
+                entity = self.robot_config["entity"]
+            self.tools_launcher.reset(entity)
         except subprocess.TimeoutExpired as e:
             self.write_to_tool_terminal(f"{e}\n\n")
             raise Exception("Failed to reset simulator")
 
         if self.robot_launcher:
             try:
-                self.robot_launcher.run()
+                self.robot_launcher.run(
+                    self.robot_config["start_pose"], self.robot_config["extra_config"]
+                )
             except Exception as e:
                 LogManager.logger.exception("Exception terminating world launcher")