Merge pull request #85 from Intelligent-Testing-Lab/79-integrate-bridge-into-agent

79 integrate bridge into agent

Author: Gwilym-Rutherford

srunner/autoagents/autoware_agent.py

@@ -13,6 +13,9 @@
 
 from std_msgs.msg import Empty
 
+from srunner.tools import ROS2_launch
+
+
 import threading
 import rclpy
 import time
@@ -57,10 +60,14 @@ def setup(self, config: EnvironmentConfig | None = None) -> None:
 
         self.sent_route = False
 
+        # launch the bridge
+        ROS2_launch.ROS2Launch.launch_file(
+            "autoware_carla_interface", "autoware_carla_interface.launch"
+        )
+
         # check the bridge is ready
         # publish sensor information to the bridge
         # wait for it to return the correct message
-
         ego_config_msg = EgoConfig()
         ego_config_msg.ego_name = config.ego_name
         ego_config_msg.ego_model = config.ego_model
@@ -148,12 +155,12 @@ def run_step(self) -> None:
                 waypoints.append(
                     self._convert_to_waypoint(waypoint).autoware_from_world_coords()
                 )
-            
+
             # autoware cannot handle many waypoints, becomes unreliable
             n_waypoints = len(waypoints)
             segment_size = int(n_waypoints / 3)
-            
-            #self.route_node.request_route(goal_pose, waypoints[0::segment_size])
+
+            # self.route_node.request_route(goal_pose, waypoints[0::segment_size])
             self.route_node.publish_route(goal_pose, waypoints[0::segment_size])
             self.sent_route = True
 

srunner/tools/ROS2_launch.py

@@ -1,54 +1,71 @@
 import subprocess
 import threading
 
+
 class ROS2Launch(object):
     threads = dict()
     stop_event = threading.Event()
-        
-    @classmethod
-    def launch_file(self, package_name: str, launch_file: str, args: dict) -> None:
+
+    @staticmethod
+    def launch_file(
+        package_name: str, launch_file: str, args: dict | None = None
+    ) -> None:
         """will launch the file specified from the package specified
 
         Args:
             package_name (str): ROS2 package name
             launch_file (str): launch file name
             args (dict): dictionary of arguments to pass to launch file
         """
-        command = self._construct_command(package_name, launch_file, args)
-        thread = threading.Thread(target=self._run_command, args=(command, self.stop_event))
-        self.threads[self.package_name] = {
-            "thread": thread,
-            "stop": False
-        }
+        command = ROS2Launch._construct_command(package_name, launch_file, args)
+        thread = threading.Thread(
+            target=ROS2Launch._run_command, args=(command, ROS2Launch.stop_event)
+        )
+        ROS2Launch.threads[package_name] = {"thread": thread, "stop": False}
         thread.start()
-    
-    @classmethod
-    def cleanup(self, package_name: str) -> None:
+
+    @staticmethod
+    def cleanup(package_name: str) -> None:
         """provide package name to stop running
 
         Args:
             package_name (str): name of the ROS2 package to kill
         """
-        if package_name in self.threads:
-            thread = self.threads[package_name]
-            thread['stop'] = True
-        
-    def _run_command(self, command: list[str], package_name) -> None:
-        process = subprocess.Popen(command, check=True)
-        
-        if package_name in self.threads:
-            while(not self.threads[package_name]['stop']):
+        if package_name in ROS2Launch.threads:
+            thread = ROS2Launch.threads[package_name]
+            thread["stop"] = True
+            thread["thread"].join()
+
+    @staticmethod
+    def _run_command(command: list[str], package_name) -> None:
+        command = (
+            f"/bin/bash && source /opt/ros/humble/setup.bash && {' '.join(command)}"
+        )
+
+        process = subprocess.Popen(
+            command,
+            shell=True,
+            stdout=subprocess.PIPE,
+            stderr=subprocess.PIPE,
+            text=True,
+        )
+
+        if package_name in ROS2Launch.threads:
+            while not ROS2Launch.threads[package_name]["stop"]:
                 if process.poll() is not None:
                     break
-                
+
             if process.poll() is None:
                 process.kill()
-        
-    def _construct_command(self, package_name, launch_file, args) -> list[str]:
+
+    @staticmethod
+    def _construct_command(
+        package_name: str, launch_file: str, args: dict | None = None
+    ) -> list[str]:
         command = []
         command.append("ros2 launch").append(package_name).append(launch_file)
-        for key, value in args.dict():
-            command.append(f"{key}:={value}")
+
+        if args is not None:
+            for key, value in args.dict():
+                command.append(f"{key}:={value}")
         return command
-            
-