Testing pick and place manager

Author: javizqh

robotics_application_manager/manager/docker_thread/docker_thread.py

@@ -20,7 +20,7 @@ def run(self):
         self.process = subprocess.Popen(
             self.cmd,
             shell=self.shell,
-            stdout=subprocess.PIPE,
+            stdout=sys.stdout,
             stderr=subprocess.PIPE,
             start_new_session=True,
             bufsize=1024,

robotics_application_manager/manager/launcher/launcher_gzsim.py

@@ -13,6 +13,10 @@
 from typing import List, Any
 from robotics_application_manager import LogManager
 
+from gz.msgs10.world_control_pb2 import WorldControl
+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}'"
@@ -162,8 +166,17 @@ def reset(self):
             "gz.msgs.WorldControl",
             "gz.msgs.Boolean",
             "3000",
-            "reset: {all: true}",
+            "reset: {model_only: true}",
         )
+
+        # node = Node()
+        # simControl_service = "/world/default/control"
+        # simControl_msg = WorldControl()
+        # simControl_msg.reset = "model_only"
+        # response = node.request(
+        #     simControl_service, simControl_msg, WorldControl, Boolean, 3000
+        # )
+
         if is_ros_service_available("/drone0/controller/_reset"):
             call_service("/drone0/controller/_reset", "std_srvs/srv/Trigger", "{}")
 

robotics_application_manager/manager/launcher/launcher_robot.py

@@ -13,60 +13,26 @@
 
 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": [],
             }
         ],
     },
+    "o3de": {},
     "physical": {},
 }
 

robotics_application_manager/manager/launcher/launcher_robot_ros2_api.py

@@ -48,31 +48,30 @@ def terminate(self):
                     thread.join()
                 self.threads.remove(thread)
 
-        kill_cmd = "pkill -9 -f "
-        cmd = kill_cmd + "spawn_robot.launch.py"
-        subprocess.call(
-            cmd,
-            shell=True,
-            stdout=subprocess.PIPE,
-            bufsize=1024,
-            universal_newlines=True,
-        )
+        # kill_cmd = "pkill -9 -f "
+        # cmd = kill_cmd + "spawn_robot_warehouse.launch.py"
+        # subprocess.call(
+        #     cmd,
+        #     shell=True,
+        #     stdout=subprocess.PIPE,
+        #     bufsize=1024,
+        #     universal_newlines=True,
+        # )
 
-        kill_cmd = "pkill -9 "
-        cmd = kill_cmd + "bridg"
-        subprocess.call(
-            cmd,
-            shell=True,
-            stdout=subprocess.PIPE,
-            bufsize=1024,
-            universal_newlines=True,
-        )
+        # cmd = kill_cmd + "/opt/ros/humble"
+        # subprocess.call(
+        #     cmd,
+        #     shell=True,
+        #     stdout=subprocess.PIPE,
+        #     bufsize=1024,
+        #     universal_newlines=True,
+        # )
 
-        cmd = kill_cmd + "robot_state_publisher"
-        subprocess.call(
-            cmd,
-            shell=True,
-            stdout=subprocess.PIPE,
-            bufsize=1024,
-            universal_newlines=True,
-        )
+        # cmd = kill_cmd + "robot_state_publisher"
+        # subprocess.call(
+        #     cmd,
+        #     shell=True,
+        #     stdout=subprocess.PIPE,
+        #     bufsize=1024,
+        #     universal_newlines=True,
+        # )

robotics_application_manager/manager/manager.py

@@ -211,6 +211,7 @@ def __init__(self, host: str, port: int):
         self.world_launcher = None
         self.world_type = None
         self.robot_launcher = None
+        self.robot_pose = None
         self.tools_launcher = None
         self.application_process = None
         self.running = True
@@ -349,7 +350,8 @@ def on_launch_world(self, event):
 
         self.robot_launcher = LauncherRobot(**cfg.model_dump())
         LogManager.logger.info(str(self.robot_launcher))
-        self.robot_launcher.run(robot_cfg["start_pose"])
+        self.robot_pose = robot_cfg["start_pose"]
+        self.robot_launcher.run(self.robot_pose)
         LogManager.logger.info("Launch transition finished")
 
     def prepare_custom_universe(self, cfg_dict):
@@ -767,6 +769,7 @@ def on_terminate_universe(self, event):
         if self.robot_launcher is not None:
             self.robot_launcher.terminate()
             self.robot_launcher = None
+            self.robot_pose = None
 
     def on_disconnect(self, event):
         """
@@ -790,6 +793,7 @@ def on_disconnect(self, event):
                 LogManager.logger.exception("Exception terminating tools launcher")
 
         if self.robot_launcher:
+            self.robot_pose = None
             try:
                 self.robot_launcher.terminate()
             except Exception as e:
@@ -873,20 +877,21 @@ def reset_sim(self):
         the appropriate ROS or Gazebo services based on the visualization type,
         and relaunches the robot if a launcher is available.
         """
-        if self.robot_launcher:
-            self.robot_launcher.terminate()
+        # if self.robot_launcher:
+        #     self.robot_launcher.terminate()
 
         try:
             self.tools_launcher.reset()
         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()
-            except Exception as e:
-                LogManager.logger.exception("Exception terminating world launcher")
+        # if self.robot_launcher:
+        #     try:
+        #         LogManager.logger.exception("Relaunching robot launcher")
+        #         self.robot_launcher.run(self.robot_pose)
+        #     except Exception as e:
+        #         LogManager.logger.exception("Exception terminating world launcher")
 
     def start(self):
         """