From f8a239c67f0a1e43efd63b50777951f3937672a3 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Fri, 5 Sep 2025 13:45:49 +0100 Subject: [PATCH 1/7] added ROS service client to send tick messages to autoware --- .../CMakeLists.txt | 15 ++++++++ srunner/autoagents/autoware_agent.py | 18 ++++++++-- .../autoagents/autoware_nodes/tick_node.py | 35 +++++++++++++------ 3 files changed, 55 insertions(+), 13 deletions(-) create mode 100644 docker/autoware_msgs/src/autoware_carla_interface_msgs/CMakeLists.txt diff --git a/docker/autoware_msgs/src/autoware_carla_interface_msgs/CMakeLists.txt b/docker/autoware_msgs/src/autoware_carla_interface_msgs/CMakeLists.txt new file mode 100644 index 0000000..61be50d --- /dev/null +++ b/docker/autoware_msgs/src/autoware_carla_interface_msgs/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_carla_interface_msgs) + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + msg/BridgeState.msg + msg/EgoConfig.msg + msg/SensorConfig.msg + srv/AutowareTick.srv + DEPENDENCIES builtin_interfaces std_msgs) + +ament_auto_package() diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 7fecbd7..66a88da 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -4,6 +4,7 @@ from srunner.autoagents.autoware_nodes import autoware_node from srunner.autoagents.autoware_nodes import route_node from srunner.autoagents.autoware_nodes import state_node +from srunner.autoagents.autoware_nodes import tick_node from srunner.autoagents.agent_state import autoware_state @@ -62,8 +63,6 @@ def setup(self, config: EnvironmentConfig | None = None) -> None: self.setup_tick_service() - self.setup_route() - def publish_sensor_state(self) -> None: # publish sensor information to the bridge # wait for it to return the correct message @@ -85,6 +84,17 @@ def publish_sensor_state(self) -> None: time.sleep(5) # DO NOT CHANGE THIS IS A MAGIC NUMBER self.state_node.ego_config_publisher.publish(ego_config_msg) + def setup_tick_service(self): + self.tick_node = tick_node.TickNode() + self._tick_executor = rclpy.executors.SingleThreadedExecutor() + + self._tick_executor.add_node(self.tick_node) + + self._executor_thread = threading.Thread( + target=self._tick_executor.spin, daemon=True + ) + self._executor_thread.start() + def set_route(self) -> None: self.agent_set_route = True @@ -150,10 +160,12 @@ def run_step(self) -> None: n_waypoints = len(waypoints) segment_size = int(n_waypoints / 3) - # 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 # check if the current route is set if self.autoware_state.route_set() and not self.autoware_state.sent_engage: self.autoware_node.publish_engage(True) + + # tick autoware + self.tick_node.autoware_tick() diff --git a/srunner/autoagents/autoware_nodes/tick_node.py b/srunner/autoagents/autoware_nodes/tick_node.py index d7bf438..646ee33 100644 --- a/srunner/autoagents/autoware_nodes/tick_node.py +++ b/srunner/autoagents/autoware_nodes/tick_node.py @@ -2,6 +2,8 @@ from rclpy.node import Node +from autoware_carla_interface_msgs.srv import AutowareTick + class TickNode(Node): """ROS2 Client Node solely responsible for ticking the Autoware-Carla-Bridge. @@ -10,22 +12,35 @@ class TickNode(Node): The node also has an optional boolean to enable tracking execution time. """ - tick_service = "/autoware/tick" + tick_service = "autoware_tick" def __init__(self, exec_time: bool = False, debug: bool = False) -> None: super().__init__("") self._exec_time = exec_time self.debug = debug - self.tick_client = self.create_client( - # message type - implement, - self.tick_service - ) + self.tick_client = self.create_client(AutowareTick, self.tick_service) + + self.get_logger().info(f"Waiting for '{self.tick_service}' service...") + while not self.tick_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info( + f"Service '{self.tick_service}' not available, waiting again..." + ) + self.get_logger().info(f"Service '{self.tick_service}' available.") + + self.req = AutowareTick.Request() def autoware_tick(self) -> None: - # get current time - # assemble tick message - # over client - # block until response received + self.req.header.frame_id = "map" + self.req.header.stamp = self.get_clock().now().to_msg() + + # send to service - blocking call + res = self.tick_client.call(self.req) - return + self.get_logger().info( + f"Service {self.tick_service} responded with delta of {res.delta / 1e6}ms" + ) + if res.delta == 0.0: + self.get_logger().info( + f"Service {self.tick_service} responded with invalid time-delta. Is CARLA running?" + ) From de2a6018333b4e8a5846be2e2870ee71e5c35c8d Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Fri, 5 Sep 2025 13:46:27 +0100 Subject: [PATCH 2/7] removed CMakeLists.txt --- .../autoware_carla_interface_msgs/CMakeLists.txt | 15 --------------- 1 file changed, 15 deletions(-) delete mode 100644 docker/autoware_msgs/src/autoware_carla_interface_msgs/CMakeLists.txt diff --git a/docker/autoware_msgs/src/autoware_carla_interface_msgs/CMakeLists.txt b/docker/autoware_msgs/src/autoware_carla_interface_msgs/CMakeLists.txt deleted file mode 100644 index 61be50d..0000000 --- a/docker/autoware_msgs/src/autoware_carla_interface_msgs/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_carla_interface_msgs) - -find_package(ament_cmake_auto REQUIRED) - -ament_auto_find_build_dependencies() - -rosidl_generate_interfaces(${PROJECT_NAME} - msg/BridgeState.msg - msg/EgoConfig.msg - msg/SensorConfig.msg - srv/AutowareTick.srv - DEPENDENCIES builtin_interfaces std_msgs) - -ament_auto_package() From 081f1ca820f2da8b94fe0c9e332a4d19297b3d69 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Fri, 5 Sep 2025 13:54:06 +0100 Subject: [PATCH 3/7] added tick log messages, changed order of ticks within SR (CARLA -> Autoware -> Scenario) --- srunner/autoagents/autoware_agent.py | 6 ++++- srunner/scenariomanager/scenario_manager.py | 12 +++------- srunner/tools/CARLA_manager.py | 25 +++++++++++++++------ 3 files changed, 26 insertions(+), 17 deletions(-) diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 66a88da..ffd385c 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -25,6 +25,7 @@ class AutowareAgent(AutonomousAgent): timestamp = None agent_set_route = False counter = 0 + last_tick = time.perf_counter_ns() def setup(self, config: EnvironmentConfig | None = None) -> None: """Setup the Autoware Agent. @@ -136,7 +137,10 @@ def run_step(self) -> None: """Tick method containing all logic based on autoware state""" self.counter += 1 if self.counter % 20 == 0: - logger.info("Ticked 1 second") + logger.info( + f"Ticked 1 second game-time, actual tick is {(time.perf_counter_ns() - self.last_tick) / 1e6}ms" + ) + self.last_tick = time.perf_counter_ns() if not self.agent_set_route: self.set_route() diff --git a/srunner/scenariomanager/scenario_manager.py b/srunner/scenariomanager/scenario_manager.py index 232a33a..d08614f 100644 --- a/srunner/scenariomanager/scenario_manager.py +++ b/srunner/scenariomanager/scenario_manager.py @@ -11,7 +11,6 @@ """ from __future__ import print_function -import sys import time import py_trees @@ -159,6 +158,9 @@ def _tick_scenario(self, timestamp): if self._debug_mode: print("\n--------- Tick ---------\n") + if self._sync_mode and self._watchdog.get_status(): + CarlaDataProvider.get_world().tick() + # Update game time and actor information GameTime.on_carla_tick(timestamp) CarlaDataProvider.on_carla_tick() @@ -169,17 +171,9 @@ def _tick_scenario(self, timestamp): # Tick scenario self.scenario_tree.tick_once() - - print("\n") - py_trees.display.print_ascii_tree(self.scenario_tree, show_status=True) - sys.stdout.flush() - if self.scenario_tree.status != py_trees.common.Status.RUNNING: self._running = False - if self._sync_mode and self._running and self._watchdog.get_status(): - CarlaDataProvider.get_world().tick() - def get_running_status(self): """ returns: diff --git a/srunner/tools/CARLA_manager.py b/srunner/tools/CARLA_manager.py index 5c089e5..6bbefe3 100644 --- a/srunner/tools/CARLA_manager.py +++ b/srunner/tools/CARLA_manager.py @@ -3,13 +3,14 @@ import time import logging -logger = logging.getLogger('scenario-runner') +logger = logging.getLogger("scenario-runner") logger.propagate = False + class CARLAManager(object): container_id = None - run_command =[ - "docker run -d --privileged --gpus all --net=host -m 16g -v /tmp/.X11-unix:/tmp/.X11-unix:rw -e DISPLAY=$DISPLAY -e NVIDIA_DRIVER_CAPABILITIES=all -e XDG_RUNTIME_DIR=/tmp carlasim/carla:0.9.15 /bin/bash -c ./CarlaUE4.sh -carla-rpc-port=3000" + run_command = [ + 'docker run -dt --gpus all --net=host -v /tmp/.X11-unix:/tmp/.X11-unix:rw -e DISPLAY=$DISPLAY -e NVIDIA_DRIVER_CAPABILITIES=all -e XDG_RUNTIME_DIR=/tmp carlasim/carla:0.9.15 /bin/bash -c "./CarlaUE4.sh -quality-level=Low"' ] @staticmethod @@ -18,7 +19,11 @@ def start_carla(): if CARLAManager.container_id is None: env = os.environ.copy() result = subprocess.run( - CARLAManager.run_command, shell=True, text=True, capture_output=True, env=env + CARLAManager.run_command, + shell=True, + text=True, + capture_output=True, + env=env, ) logger.info(f"Started CARLA container {result.stdout.strip()}") else: @@ -29,14 +34,20 @@ def stop_carla(): if CARLAManager.container_id is not None: env = os.environ.copy() result = subprocess.run( - f"docker container kill {CARLAManager.container_id}", shell=True, text=True, capture_output=True, env=env + f"docker container kill {CARLAManager.container_id}", + shell=True, + text=True, + capture_output=True, + env=env, + ) + logger.info( + f"Killed CARLA container {CARLAManager.container_id} with exit code {result.returncode}" ) - logger.info(f"Killed CARLA container {CARLAManager.container_id} with exit code {result.returncode}") logger.info(f"Kill stdout: {result.stdout.strip()}") CARLAManager.container_id = None @staticmethod def restart_carla(): CARLAManager.stop_carla() - time.sleep(10) + time.sleep(5) CARLAManager.start_carla() From 82275346b6bef6b4d918733e403dc91f25ec34d8 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Mon, 8 Sep 2025 15:36:14 +0100 Subject: [PATCH 4/7] added loop to initialise agent -> fixed tick budget to ensure determinism --- aw_scenario_runner.py | 63 +++++++++++++------ config.yaml | 4 +- srunner/autoagents/autonomous_agent.py | 9 +++ srunner/autoagents/autoware_agent.py | 32 +++++++--- .../autoagents/autoware_nodes/tick_node.py | 9 +-- srunner/scenarios/route_scenario.py | 9 +-- srunner/tools/CARLA_manager.py | 33 ++++++++-- 7 files changed, 115 insertions(+), 44 deletions(-) diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index 3227f8c..80a50e4 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -14,6 +14,8 @@ import carla +import time + from srunner.scenariomanager.scenario_manager import ScenarioManager from srunner.tools.results_manager import ScenarioDefinitionManager from srunner.scenarios.route_scenario import RouteScenario @@ -24,9 +26,12 @@ from srunner.scenarioconfigs.route_scenario_configuration import ( RouteScenarioConfiguration, ) +from srunner.tools import route_manipulation from srunner.objects.ego_vehicle import EgoVehicle from srunner.tools.log import LogUtil +from srunner.tools.CARLA_manager import CARLAManager + logger = logging.getLogger("scenario-runner") @@ -131,15 +136,6 @@ def run_scenario( logger.info(f"{settings.__str__()}") - if self._tm_config["active"]: - logger.info("Loading Traffic Manager...") - tm_port = int(self._tm_config["port"]) # type: ignore - CarlaDataProvider.set_traffic_manager_port(tm_port) - tm = self.carla_client.get_trafficmanager(tm_port) - - tm.set_random_device_seed(int(self._tm_config["seed"])) # ADD TO CONFIG - tm.set_synchronous_mode(self._tm_config["sync"]) - # update the world CarlaDataProvider.set_world(self.carla_world) @@ -150,6 +146,12 @@ def run_scenario( self.carla_world.tick() # client must tick to spawn actors + ego.prepare_ego() + + self.carla_world.tick() + + logger.info("Initialising Autoware...") + if not self.DEV_MODE: logger.info("Loading Autoware agent") agent_class_name = self.module_aw_agent.__name__.title().replace("_", "") @@ -165,29 +167,47 @@ def run_scenario( result = False return - ego.prepare_ego() - logger.info("Loading route...") + gps_route, route = route_manipulation.interpolate_trajectory( + route_config.keypoints + ) + route_config.agent.set_global_plan(gps_route, route) # set agent route + + logger.info("Initialising agent route...") + + # allow the agent to localise and set the route + budget = int(self._scenario_config["initialisation_budget"]) + status = False # completion status + for tick in range(0, budget): + self.carla_world.tick() + status = route_config.agent.run_step_init() # type: ignore + + if status: + logger.info(f"Successfully initialised agent in {tick} ticks") + break + + if self._tm_config["active"]: + logger.info("Loading Traffic Manager...") + tm_port = int(self._tm_config["port"]) # type: ignore + CarlaDataProvider.set_traffic_manager_port(tm_port) + tm = self.carla_client.get_trafficmanager(tm_port) + + tm.set_random_device_seed(int(self._tm_config["seed"])) # ADD TO CONFIG + tm.set_synchronous_mode(self._tm_config["sync"]) + try: # the route gets sent to the agent here scenario = RouteScenario( world=self.carla_world, config=route_config, debug_mode=self.DEBUG, ego_vehicle=ego._actor, + route=route, ) except Exception: logger.info("Could not load Route Scenario") traceback.print_exc() - # need to tick autoware and CARLA - # a determined number of times - # to allow it to plan the route - # assign a 'tick' budget - # exceeding budget = failure - # call agent init function or something... - # no need to tick scenario, just CARLA - logger.info("Starting scenario...") try: # recorder_name = f"{self.results_manager.last_scenario}/recording.log" @@ -249,7 +269,8 @@ def run(self) -> None: for iteration in range(self.iterations): logger.info("Starting CARLA container....") - # CARLAManager.start_carla() + CARLAManager.restart_carla() + time.sleep(5) self.curr_iteration = iteration logger.info(f"Starting algorithm iteration number {self.curr_iteration}") @@ -401,6 +422,8 @@ def main(): logger.addHandler(fh) logger.addHandler(sh) + CARLAManager._load_config(config["carla"]) + # reload world and sync must be present when running agent-based route scenarios scenario_runner = None try: diff --git a/config.yaml b/config.yaml index 26bbbd7..e848422 100644 --- a/config.yaml +++ b/config.yaml @@ -4,7 +4,8 @@ log: log_format: '[%(levelname)s] [%(asctime)s] [%(message)s] [%(filename)s]:[%(lineno)d]' carla: host: 127.0.0.1 - port: 3000 + fidelity: 'Low' + port: 2000 timeout: 20 sync: true fixed_delta_seconds: 0.05 # update rate 1 / FPS @@ -19,6 +20,7 @@ scenario_runner: json: ./example_scenario.json route_id: 0 agent: srunner/autoagents/autoware_agent + initialisation_budget: 300 # ticks algorithm: iterations: 10 path: algorithms/test_alg # relative path diff --git a/srunner/autoagents/autonomous_agent.py b/srunner/autoagents/autonomous_agent.py index 7980efd..59a1407 100644 --- a/srunner/autoagents/autonomous_agent.py +++ b/srunner/autoagents/autonomous_agent.py @@ -57,6 +57,15 @@ def sensors(self): # pylint: disable=no-self-use return sensors + def run_step_init(self): + """ + Override + Initialisation loop / parameters + + :return: status (ready or not) + """ + return + def run_step(self): """ Override diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index ffd385c..28b41f0 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -65,8 +65,6 @@ def setup(self, config: EnvironmentConfig | None = None) -> None: self.setup_tick_service() def publish_sensor_state(self) -> None: - # publish sensor information to the bridge - # wait for it to return the correct message ego_config_msg = EgoConfig() ego_config_msg.ego_name = self.config.ego_name # type: ignore ego_config_msg.ego_model = self.config.ego_model # type: ignore @@ -133,15 +131,13 @@ def cleanup(self) -> None: except RuntimeError: logger.info("Failed to clean up executor thread...") - def run_step(self) -> None: - """Tick method containing all logic based on autoware state""" - self.counter += 1 - if self.counter % 20 == 0: - logger.info( - f"Ticked 1 second game-time, actual tick is {(time.perf_counter_ns() - self.last_tick) / 1e6}ms" - ) - self.last_tick = time.perf_counter_ns() + def run_step_init(self) -> bool: + """Route Initialisation loop + + Ticks CARLA and Autoware, allowing the agent to localise and plan the route. + Operates on a fixed tick budget to ensure determinism. If the agent goes over the budget, it is treated as a failure. + """ if not self.agent_set_route: self.set_route() @@ -167,6 +163,22 @@ def run_step(self) -> None: self.route_node.publish_route(goal_pose, waypoints[0::segment_size]) self.sent_route = True + # check if the current route is set and we are able to send engage + if self.autoware_state.route_set() and not self.autoware_state.sent_engage: + return True + + self.tick_node.autoware_tick() + return False + + def run_step(self) -> None: + """Tick method containing all logic based on autoware state""" + self.counter += 1 + if self.counter % 20 == 0: + logger.info( + f"Ticked 1 second game-time, actual tick is {(time.perf_counter_ns() - self.last_tick) / 1e6}ms" + ) + self.last_tick = time.perf_counter_ns() + # check if the current route is set if self.autoware_state.route_set() and not self.autoware_state.sent_engage: self.autoware_node.publish_engage(True) diff --git a/srunner/autoagents/autoware_nodes/tick_node.py b/srunner/autoagents/autoware_nodes/tick_node.py index 646ee33..9db7f5e 100644 --- a/srunner/autoagents/autoware_nodes/tick_node.py +++ b/srunner/autoagents/autoware_nodes/tick_node.py @@ -15,7 +15,7 @@ class TickNode(Node): tick_service = "autoware_tick" def __init__(self, exec_time: bool = False, debug: bool = False) -> None: - super().__init__("") + super().__init__("tick_node_client") self._exec_time = exec_time self.debug = debug @@ -37,9 +37,10 @@ def autoware_tick(self) -> None: # send to service - blocking call res = self.tick_client.call(self.req) - self.get_logger().info( - f"Service {self.tick_service} responded with delta of {res.delta / 1e6}ms" - ) + if self.debug: + self.get_logger().info( + f"Service {self.tick_service} responded with delta of {res.delta}ms" + ) if res.delta == 0.0: self.get_logger().info( f"Service {self.tick_service} responded with invalid time-delta. Is CARLA running?" diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index a478aa4..fe77c97 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -46,7 +46,6 @@ ) from srunner.scenarios.basic_scenario import BasicScenario -from srunner.scenarios.background_activity import BackgroundBehavior from srunner.scenariomanager.weather_sim import RouteWeatherBehavior from srunner.scenariomanager.lights_sim import RouteLightsBehavior from srunner.scenariomanager.timer import RouteTimeoutBehavior @@ -76,13 +75,15 @@ def __init__( criteria_enable=True, timeout=300, ego_vehicle=None, + route=None, ): """ Setup all relevant parameters and create scenarios along route """ self.config = config - self.route = self._get_route(config) + self.route = self._get_route(config) if not route else route + sampled_scenario_definitions = self._filter_scenarios(config.scenario_configs) if not ego_vehicle: @@ -380,11 +381,11 @@ def _create_behavior(self): ) # Tick the ScenarioTriggerer before the scenarios # Add the Background Activity - #behavior.add_child( + # behavior.add_child( # BackgroundBehavior( # self.ego_vehicles[0], self.route, name="BackgroundActivity" # ) - #) + # ) behavior.add_children(scenario_behaviors) return behavior diff --git a/srunner/tools/CARLA_manager.py b/srunner/tools/CARLA_manager.py index 6bbefe3..1ea9e18 100644 --- a/srunner/tools/CARLA_manager.py +++ b/srunner/tools/CARLA_manager.py @@ -1,6 +1,5 @@ import subprocess import os -import time import logging logger = logging.getLogger("scenario-runner") @@ -9,10 +8,21 @@ class CARLAManager(object): container_id = None + port = 2000 # default + fidelity = "Low" # default run_command = [ - 'docker run -dt --gpus all --net=host -v /tmp/.X11-unix:/tmp/.X11-unix:rw -e DISPLAY=$DISPLAY -e NVIDIA_DRIVER_CAPABILITIES=all -e XDG_RUNTIME_DIR=/tmp carlasim/carla:0.9.15 /bin/bash -c "./CarlaUE4.sh -quality-level=Low"' + 'docker run -dt --gpus all --net=host -v /tmp/.X11-unix:/tmp/.X11-unix:rw -e DISPLAY=$DISPLAY -e NVIDIA_DRIVER_CAPABILITIES=all -e XDG_RUNTIME_DIR=/tmp carlasim/carla:0.9.15 /bin/bash -c "./CarlaUE4.sh -carla-rpc-port=2000 -quality-level=Low"' ] + @staticmethod + def _load_config(config: dict) -> None: + CARLAManager.port = config["port"] + CARLAManager.fidelity = config["fidelity"] + + CARLAManager.run_command = [ + f'docker run -dt --gpus all --net=host -v /tmp/.X11-unix:/tmp/.X11-unix:rw -e DISPLAY=$DISPLAY -e NVIDIA_DRIVER_CAPABILITIES=all -e XDG_RUNTIME_DIR=/tmp carlasim/carla:0.9.15 /bin/bash -c "./CarlaUE4.sh -carla-rpc-port={CARLAManager.port} -quality-level={CARLAManager.fidelity}"' + ] + @staticmethod def start_carla(): # need a way to verify CARLA has launched @@ -26,6 +36,7 @@ def start_carla(): env=env, ) logger.info(f"Started CARLA container {result.stdout.strip()}") + CARLAManager.container_id = result.stdout.strip() else: CARLAManager.restart_carla() @@ -48,6 +59,18 @@ def stop_carla(): @staticmethod def restart_carla(): - CARLAManager.stop_carla() - time.sleep(5) - CARLAManager.start_carla() + if CARLAManager.container_id is not None: + env = os.environ.copy() + result = subprocess.run( + f"docker container restart {CARLAManager.container_id}", + shell=True, + text=True, + capture_output=True, + env=env, + ) + logger.info( + f"Restarted CARLA container {CARLAManager.container_id} with exit code {result.returncode}" + ) + CARLAManager.container_id = result.stdout.strip() + else: + CARLAManager.start_carla() From 4d7f43fb999631ea1f7eeeaefd9a3a59542adc82 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Mon, 8 Sep 2025 16:06:55 +0100 Subject: [PATCH 5/7] spawning ego at route start instead of dedicated spawn point --- aw_scenario_runner.py | 10 +++------- srunner/objects/ego_vehicle.py | 18 +++--------------- srunner/scenarios/route_scenario.py | 10 +--------- 3 files changed, 7 insertions(+), 31 deletions(-) diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index 80a50e4..2ad6cd6 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -146,10 +146,6 @@ def run_scenario( self.carla_world.tick() # client must tick to spawn actors - ego.prepare_ego() - - self.carla_world.tick() - logger.info("Initialising Autoware...") if not self.DEV_MODE: @@ -174,6 +170,9 @@ def run_scenario( ) route_config.agent.set_global_plan(gps_route, route) # set agent route + ego.prepare_ego(route[0][0]) # set location to first waypoint + + self.carla_world.tick() logger.info("Initialising agent route...") # allow the agent to localise and set the route @@ -399,9 +398,6 @@ def _cleanup(self) -> None: def main(): - # single argument of configuration file - - # configure logger config = None with open("config.yaml", "r") as stream: config = yaml.safe_load(stream) diff --git a/srunner/objects/ego_vehicle.py b/srunner/objects/ego_vehicle.py index 2523908..3cd54fc 100644 --- a/srunner/objects/ego_vehicle.py +++ b/srunner/objects/ego_vehicle.py @@ -44,22 +44,10 @@ def spawn(self) -> carla.Actor: return self._actor - def setup_sensors(self) -> None: - """Spawns and attatches the necessary sensors to the EgoVehcicle.""" - if self._actor is None: - logger.error( - "EgoVehicle has no carla.Actor, it probably failed to spawn. Check the spawn coordinates, and ensure you call EgoVehicle.spawn() first." - ) - - bp_library = CarlaDataProvider.get_world().get_blueprint_library() - - for sensor in self._env.sensor_config: - sensor._spawn(bp_library, self._actor) - logger.info(f"Spawned {sensor.type} attatched to {self.ego_name}") - - def prepare_ego(self) -> None: + def prepare_ego(self, route_loc: carla.Transform) -> None: """Reset the position and velocity of the ego. Register the actor""" - self._actor.set_transform(self._env.ego_spawn) + route_loc.location.z += 0.5 + self._actor.set_transform(route_loc) self._actor.set_target_velocity(carla.Vector3D()) self._actor.set_target_angular_velocity(carla.Vector3D()) diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index fe77c97..8598b19 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -88,8 +88,7 @@ def __init__( if not ego_vehicle: ego_vehicle = self._spawn_ego_vehicle() - else: - self._update_ego_pos(ego_vehicle) + self.timeout = self._estimate_route_timeout() if debug_mode: @@ -171,13 +170,6 @@ def _spawn_ego_vehicle(self): return ego_vehicle - def _update_ego_pos(self, ego: carla.Actor) -> None: - """Moves the ego vehicle to the start position""" - elevate_transform = self.route[0][0] - elevate_transform.location.z += 0.5 - - ego.set_transform(elevate_transform) - def _estimate_route_timeout(self): """ Estimate the duration of the route, as a proportinal value of its length From 24bc7695df2027199c7b6e63757325c7388f8fc6 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Mon, 8 Sep 2025 16:51:50 +0100 Subject: [PATCH 6/7] added spectator camera and option to follow --- aw_scenario_runner.py | 6 ++-- config.yaml | 3 +- srunner/scenariomanager/scenario_manager.py | 32 ++++++++++++++++----- 3 files changed, 30 insertions(+), 11 deletions(-) diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index 2ad6cd6..0264de4 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -184,7 +184,6 @@ def run_scenario( if status: logger.info(f"Successfully initialised agent in {tick} ticks") - break if self._tm_config["active"]: logger.info("Loading Traffic Manager...") @@ -211,8 +210,9 @@ def run_scenario( try: # recorder_name = f"{self.results_manager.last_scenario}/recording.log" # self.carla_client.start_recorder(recorder_name, True) - - self.scenario_manager.load_scenario(scenario, self.aw_agent) + self.scenario_manager.load_scenario( + scenario, self.aw_agent, follow_ego=self._scenario_config["follow_ego"] + ) self.scenario_manager.run_scenario() # self.carla_client.stop_recorder() diff --git a/config.yaml b/config.yaml index e848422..a3b52ad 100644 --- a/config.yaml +++ b/config.yaml @@ -17,10 +17,11 @@ traffic_manager: scenario_runner: debug: false dev_mode: false + follow_ego: false # camera to follow ego vehicle json: ./example_scenario.json route_id: 0 agent: srunner/autoagents/autoware_agent - initialisation_budget: 300 # ticks + initialisation_budget: 200 # ticks algorithm: iterations: 10 path: algorithms/test_alg # relative path diff --git a/srunner/scenariomanager/scenario_manager.py b/srunner/scenariomanager/scenario_manager.py index d08614f..3a38627 100644 --- a/srunner/scenariomanager/scenario_manager.py +++ b/srunner/scenariomanager/scenario_manager.py @@ -15,6 +15,8 @@ import py_trees +import carla + from srunner.autoagents.agent_wrapper import AgentWrapper from srunner.scenariomanager.carla_data_provider import CarlaDataProvider from srunner.scenariomanager.result_writer import ResultOutputProvider @@ -47,6 +49,7 @@ def __init__(self, debug_mode=False, sync_mode=False, timeout=2.0): self.scenario = None self.scenario_tree = None self.ego_vehicles = None + self.follow_ego = None self.other_actors = None self._debug_mode = debug_mode @@ -92,7 +95,7 @@ def cleanup(self): CarlaDataProvider.cleanup() - def load_scenario(self, scenario, agent=None): + def load_scenario(self, scenario, agent=None, follow_ego=False): """ Load a new scenario """ @@ -103,14 +106,13 @@ def load_scenario(self, scenario, agent=None): self.scenario = scenario self.scenario_tree = self.scenario.scenario_tree self.ego_vehicles = scenario.ego_vehicles + self.follow_ego = follow_ego self.other_actors = scenario.other_actors - # To print the scenario tree uncomment the next line - # py_trees.display.render_dot_tree(self.scenario_tree) - - # no need to setup sensors, done by autoware bridge - # if self._agent is not None: - # self._agent.setup_sensors(self.ego_vehicles[0], self._debug_mode) + if follow_ego: + self.world_cam = CarlaDataProvider.get_world().get_spectator() + self._camera_offset = carla.Location(x=-5, y=0, z=15) + self._camera_pitch = -60.0 # degrees def run_scenario(self): """ @@ -171,9 +173,25 @@ def _tick_scenario(self, timestamp): # Tick scenario self.scenario_tree.tick_once() + if self.follow_ego: + self._tick_spectator_cam(self.ego_vehicles[0]) # type: ignore + if self.scenario_tree.status != py_trees.common.Status.RUNNING: self._running = False + def _tick_spectator_cam(self, ego: carla.Actor) -> None: + """Ticks the spectator camera for the chosen ego""" + vehicle_transform = ego.get_transform() + delta_spec_loc = vehicle_transform.location + self._camera_offset + + delta_spec_trans = carla.Transform( + delta_spec_loc, + carla.Rotation( + pitch=self._camera_pitch, yaw=vehicle_transform.rotation.yaw, roll=0 + ), + ) + self.world_cam.set_transform(delta_spec_trans) + def get_running_status(self): """ returns: From 11021eaa8e32b20213ba804ca33b2fc8534eb079 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Tue, 9 Sep 2025 13:34:33 +0100 Subject: [PATCH 7/7] updated docker cp to host --- .../{hill_climbing.py => hill_climb.py} | 24 +++++-- aw_scenario_runner.py | 68 +++++++++++++++---- config.yaml | 11 +-- requirements.txt | 1 + srunner/scenarios/route_scenario.py | 15 ++-- srunner/tools/CARLA_manager.py | 19 ++++++ 6 files changed, 108 insertions(+), 30 deletions(-) rename algorithms/{hill_climbing.py => hill_climb.py} (79%) diff --git a/algorithms/hill_climbing.py b/algorithms/hill_climb.py similarity index 79% rename from algorithms/hill_climbing.py rename to algorithms/hill_climb.py index a7dd625..0dbc1a5 100644 --- a/algorithms/hill_climbing.py +++ b/algorithms/hill_climb.py @@ -1,11 +1,12 @@ from basic_algorithm import BasicAlgorithm import lanelet2 import random +import json from math import sqrt, pow -class Hill_Climb(BasicAlgorithm): +class HillClimb(BasicAlgorithm): def __init__(self, args: dict) -> None: super(BasicAlgorithm).__init__() self.radius = args["radius"] @@ -20,7 +21,7 @@ def _scenario_callback( self, scenario_definition: dict, driving_score: float ) -> dict: # pass in route id - waypoints = scenario_definition["routes"][0]["waypoints"] + waypoints = scenario_definition["routes"][0]["route"]["waypoints"] if self.prev_ds is not None: if not driving_score >= self.prev_ds: @@ -78,9 +79,24 @@ def __get_all_lanelet_points(self) -> set[tuple[int, int]]: centerline_points = [] for lanelet in list(lanelets): for points in lanelet.centerline: - centerline_points += (points.x, points.y) + centerline_points.append((points.x, points.y)) return set(centerline_points) def __euclidian_distance(self, point1, point2) -> float: - return sqrt(pow(point1.x - point2.x, 2) + pow(point2.y - point1.x, 2)) + return sqrt(pow(point1[0] - point2[0], 2) + pow(point2[1] - point1[1], 2)) + + +def main(): # debug + scenario = "/autoware_scenario_runner/example_scenario_default.json" + + json_scenario = None + with open(scenario, "r") as f: + json_scenario = json.load(f) + + radius = 10 + lanelet2_path = "/autoware_scenario_runner/algorithms/resources/Town01.osm" + args = {"radius": radius, "lanelet_path": lanelet2_path} + + climb = HillClimb(args) + climb._scenario_callback(json_scenario, 0.0) diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index 0264de4..49a7aea 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -34,6 +34,15 @@ logger = logging.getLogger("scenario-runner") +infractions_dict = { + "OutsideRouteLanesTest": 0.3, + "CollisionTest": 1.0, + "RunningRedLightTest": 0.4, + "RunningStopTest": 0.25, +} + +terminations_dict = {"AgentBlockedTest": 0.0} + class AWScenarioRunner(object): # flags @@ -50,6 +59,7 @@ class AWScenarioRunner(object): definition_manager = None aw_agent = None + host_volume = os.environ["SR_HOST_VOLUME"] def __init__(self, config: dict) -> None: """ @@ -178,12 +188,14 @@ def run_scenario( # allow the agent to localise and set the route budget = int(self._scenario_config["initialisation_budget"]) status = False # completion status - for tick in range(0, budget): + for tick in range(1, budget + 1): self.carla_world.tick() status = route_config.agent.run_step_init() # type: ignore - if status: - logger.info(f"Successfully initialised agent in {tick} ticks") + if not status: + logger.info("Agent failed to initialise route.") + else: + logger.info("Successfully initialised agent; route set.") if self._tm_config["active"]: logger.info("Loading Traffic Manager...") @@ -194,7 +206,7 @@ def run_scenario( tm.set_random_device_seed(int(self._tm_config["seed"])) # ADD TO CONFIG tm.set_synchronous_mode(self._tm_config["sync"]) - try: # the route gets sent to the agent here + try: scenario = RouteScenario( world=self.carla_world, config=route_config, @@ -208,14 +220,12 @@ def run_scenario( logger.info("Starting scenario...") try: - # recorder_name = f"{self.results_manager.last_scenario}/recording.log" - # self.carla_client.start_recorder(recorder_name, True) + self.carla_client.start_recorder("/home/carla/recording.log", True) self.scenario_manager.load_scenario( scenario, self.aw_agent, follow_ego=self._scenario_config["follow_ego"] ) self.scenario_manager.run_scenario() - - # self.carla_client.stop_recorder() + self.carla_client.stop_recorder() result = True except Exception: traceback.print_exc() @@ -269,7 +279,7 @@ def run(self) -> None: for iteration in range(self.iterations): logger.info("Starting CARLA container....") CARLAManager.restart_carla() - time.sleep(5) + time.sleep(5) # allow CARLA to load self.curr_iteration = iteration logger.info(f"Starting algorithm iteration number {self.curr_iteration}") @@ -297,7 +307,7 @@ def run(self) -> None: scenario_result.put(result_dict) scenario_process = multiprocessing.Process( - target=self.run_scenario, + target=self.run_scenario, # need to catch connection exception args=( route_config, env_config, @@ -316,7 +326,18 @@ def run(self) -> None: if scenario_process.is_alive(): scenario_process.kill() + # copy over the recording from CARLA container if env variable is setup + if self.host_volume is not None: + CARLAManager.fetch_file( + "/home/carla/recording.log", + f"{self.host_volume}/{self.results_manager.last_scenario}/recording.log", + ) + + logger.info("Calculating driving score...") driving_score = self._calculate_driving_score(result["criteria"]) + logger.info( + f"Scenario iteration {iteration} achieved a score of {driving_score}" + ) # read the scenario definition if not self.DEV_MODE: @@ -352,8 +373,31 @@ def _output_criteria( return criteria_dict def _calculate_driving_score(self, criteria: dict) -> float: - # to be implemented - return 0.0 + driving_score = 0.0 + + for key in terminations_dict.keys(): + if not criteria[key]["success_value"] == criteria[key]["actual_value"]: + logger.info(f"Found terminal condition {key}.") + return 0.0 # hit a termination condition, driving score of 0.0 + + completed_route = float(criteria["RouteCompletionTest"]["actual_value"]) / 100 + logger.info(f"Agent route completion: {completed_route * 100}%") + + logger.info("Checking penality conditions...") + penalties = 1 + for infraction, penalty in infractions_dict.items(): + delta_penalty = float(criteria[infraction]["actual_value"] * penalty) + + if delta_penalty: + logger.info( + f"Condition {infraction}: Breached {criteria[infraction]['actual_value']} times" + ) + logger.info(f"Applying penalty of {delta_penalty}") + else: + logger.info(f"Condition {infraction}: Found zero breaches") + + driving_score = completed_route * (1 / penalties) + return driving_score def destroy(self) -> None: """Deletes instances of all classes related to CARLA""" diff --git a/config.yaml b/config.yaml index a3b52ad..945dc24 100644 --- a/config.yaml +++ b/config.yaml @@ -1,7 +1,7 @@ log: path: 'logs/' clear_old_logs: true - log_format: '[%(levelname)s] [%(asctime)s] [%(message)s] [%(filename)s]:[%(lineno)d]' + log_format: '[%(levelname)s] [%(created)f] [%(filename)s] %(message)s' carla: host: 127.0.0.1 fidelity: 'Low' @@ -10,20 +10,21 @@ carla: sync: true fixed_delta_seconds: 0.05 # update rate 1 / FPS traffic_manager: - active: false + active: true sync: true # must be the same as carla sync seed: 0 port: 8001 scenario_runner: debug: false dev_mode: false - follow_ego: false # camera to follow ego vehicle + follow_ego: true json: ./example_scenario.json route_id: 0 agent: srunner/autoagents/autoware_agent initialisation_budget: 200 # ticks algorithm: iterations: 10 - path: algorithms/test_alg # relative path + path: algorithms/test_alg args: # will be passed into the algorithm class as a dict - test_arg: 10 + lanelet_path: /autoware_scenario_runner/algorithms/resources/Town01.osm + radius: 10 diff --git a/requirements.txt b/requirements.txt index a5ccc9a..f982bbd 100644 --- a/requirements.txt +++ b/requirements.txt @@ -13,3 +13,4 @@ six simple-watchdog-timer antlr4-python3-runtime==4.10 graphviz +lanelet2 diff --git a/srunner/scenarios/route_scenario.py b/srunner/scenarios/route_scenario.py index 8598b19..6a93e1f 100644 --- a/srunner/scenarios/route_scenario.py +++ b/srunner/scenarios/route_scenario.py @@ -46,6 +46,7 @@ ) from srunner.scenarios.basic_scenario import BasicScenario +from srunner.scenarios.background_activity import BackgroundBehavior from srunner.scenariomanager.weather_sim import RouteWeatherBehavior from srunner.scenariomanager.lights_sim import RouteLightsBehavior from srunner.scenariomanager.timer import RouteTimeoutBehavior @@ -63,10 +64,6 @@ class RouteScenario(BasicScenario): along which several smaller scenarios are triggered """ - # fix route scenario - # take in a pre-spawned ego vehicle - # position it at the first waypoint in the route - def __init__( self, world, @@ -373,11 +370,11 @@ def _create_behavior(self): ) # Tick the ScenarioTriggerer before the scenarios # Add the Background Activity - # behavior.add_child( - # BackgroundBehavior( - # self.ego_vehicles[0], self.route, name="BackgroundActivity" - # ) - # ) + behavior.add_child( + BackgroundBehavior( + self.ego_vehicles[0], self.route, name="BackgroundActivity" + ) + ) behavior.add_children(scenario_behaviors) return behavior diff --git a/srunner/tools/CARLA_manager.py b/srunner/tools/CARLA_manager.py index 1ea9e18..4b88d95 100644 --- a/srunner/tools/CARLA_manager.py +++ b/srunner/tools/CARLA_manager.py @@ -57,6 +57,25 @@ def stop_carla(): logger.info(f"Kill stdout: {result.stdout.strip()}") CARLAManager.container_id = None + @staticmethod + def fetch_file(path: str, dest: str): + if CARLAManager.container_id is not None: + env = os.environ.copy() + result = subprocess.run( + f"docker cp {CARLAManager.container_id}:{path} {dest}", + shell=True, + text=True, + capture_output=True, + env=env, + ) + logger.info( + f"Copying... {path} from container {CARLAManager.container_id} to {dest}" + ) + logger.info(f"docker cp {CARLAManager.container_id}:{path} {dest}") + + if not result.returncode == 0: + logger.info(f"Failed to copy {path} to {dest}") + @staticmethod def restart_carla(): if CARLAManager.container_id is not None: