diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index accb446..0f0cbe6 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/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 diff --git a/srunner/tools/ROS2_launch.py b/srunner/tools/ROS2_launch.py index 695a437..7845dc6 100644 --- a/srunner/tools/ROS2_launch.py +++ b/srunner/tools/ROS2_launch.py @@ -1,12 +1,15 @@ 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: @@ -14,41 +17,55 @@ def launch_file(self, package_name: str, launch_file: str, args: dict) -> None: 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 - - \ No newline at end of file