From d8db51559ca1cfb1f47520eba99661b188e104ea Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Wed, 10 Sep 2025 15:36:26 +0100 Subject: [PATCH 1/9] implemented new random search algorithm; fixed MetricsCollector race condition when accessing MetricsCollector.fetch_state() --- srunner/tools/metrics_collector.py | 106 +++++++++++++++++++---------- 1 file changed, 71 insertions(+), 35 deletions(-) diff --git a/srunner/tools/metrics_collector.py b/srunner/tools/metrics_collector.py index 1471ef2..5e23306 100644 --- a/srunner/tools/metrics_collector.py +++ b/srunner/tools/metrics_collector.py @@ -4,6 +4,11 @@ import threading import json import time +import os +import logging + +logger = logging.getLogger("scenario-runner") +logger.propagate = False class MetricsCollector: @@ -15,6 +20,9 @@ class MetricsCollector: _file_target = "" _thread = None _running = False + state_queue = Queue(maxsize=0) + _state_lock = threading.Lock() + _flush_freq = 100 @classmethod def init_state(cls, state: dict, file_target: str, include: bool = False) -> None: @@ -25,8 +33,8 @@ def init_state(cls, state: dict, file_target: str, include: bool = False) -> Non file_target (str): target file include (bool, optional): write the initial state. Defaults to False. """ - cls.state = state.copy() - cls.state_queue = Queue(maxsize=0) # infinite queue + with cls._state_lock: + cls.state = state.copy() if include: cls.state_queue.put_nowait(state.copy()) @@ -37,13 +45,14 @@ def init_state(cls, state: dict, file_target: str, include: bool = False) -> Non @classmethod def reset(cls) -> None: """Resets the state of the class. Must be initialised again.""" - if cls._running and cls._thread is not None: - cls._running = False - cls._thread.join() + if cls._running: + cls.stop_thread() - cls.state = {} + with cls._state_lock: + cls.state = {} cls._file_target = "" cls._thread = None + cls.state_queue = Queue(maxsize=0) @classmethod def update_key(cls, key: str, value: Any) -> None: @@ -54,57 +63,83 @@ def update_key(cls, key: str, value: Any) -> None: key (str): dict key value (Any): value """ - - if key in cls.state.keys(): - cls.state[key] = value + with cls._state_lock: + if key in cls.state: + cls.state[key] = value @classmethod def save_state(cls) -> None: """Push the current state into the Queue""" if cls._running: - state_cp = cls.state.copy() + with cls._state_lock: + state_cp = cls.state.copy() cls.state_queue.put_nowait(state_cp) @classmethod def fetch_key(cls, key: str) -> Any: """Get the value of a key if it exists""" - if cls._running and key in cls.state.keys(): - return cls.state[key] + if cls._running: + with cls._state_lock: + if key in cls.state: + return cls.state[key] + return None @classmethod def fetch_state(cls) -> Any: """Return the current state""" if cls._running: - return cls.state.copy() + with cls._state_lock: + return cls.state.copy() + return {} @classmethod def _start_thread(cls) -> None: - cls._running = True - cls._thread = threading.Thread(target=cls._thread_target) - cls._thread.start() + if not cls._running: + cls._running = True + cls._thread = threading.Thread(target=cls._thread_target) + cls._thread.start() @classmethod def stop_thread(cls) -> None: - cls._running = False - cls._thread.join() # type: ignore + """Signals the thread to stop and waits for it to finish.""" + if cls._running and cls._thread is not None: + cls._running = False + cls._thread.join() @classmethod def _thread_target(cls) -> None: - f = open(cls._file_target, "w") - - f.write("[") - while cls._running: - try: - state = cls.state_queue.get( - block=True, timeout=0.05 - ) # can't block thread as it will never finish - serialized_json = json.dumps(state) - f.write(serialized_json + ",") - except Empty: - pass - - f.write("]") # last json character - f.close() + # flush every 100 pushes + _pos = 0 + try: + with open(cls._file_target, "w") as f: + f.write("[") + is_first_item = True + + while cls._running or not cls.state_queue.empty(): + try: + state = cls.state_queue.get(block=True, timeout=0.05) + + if not is_first_item: + f.write(",") + + json.dump(state, f) + is_first_item = False + + print("Pushed") + _pos += 1 + + if _pos % cls._flush_freq == 0: + print("Flushing buffer") + start = time.perf_counter() + f.flush() + os.fsync(f.fileno()) + print(f"Flushing took {time.perf_counter() - start}ms") + except Empty: + # Queue was empty, just continue the loop to check cls._running again + pass + f.write("]") + except Exception as e: + logger.error(f"Error in MetricsCollector thread: {e}") if __name__ == "__main__": @@ -116,11 +151,12 @@ def _thread_target(cls) -> None: MetricsCollector.init_state(state, target, include=False) - iterations = 10 + iterations = 1000 for i in range(1, iterations + 1): - time.sleep(1) # 1 second intervals + time.sleep(0.01) # 1 second intervals # update state MetricsCollector.update_key("timestamp", time.perf_counter()) + state = MetricsCollector.fetch_state() MetricsCollector.update_key("value", i) MetricsCollector.save_state() From 140de7d3cea2375accaef74a60740f51a4dedb64 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Wed, 10 Sep 2025 15:40:01 +0100 Subject: [PATCH 2/9] RandomSearch algorithm --- .vscode/settings.json | 4 +++ algorithms/hill_climb.py | 8 +++--- algorithms/random_search.py | 54 +++++++++++++++++++++++++++++++++++++ aw_scenario_runner.py | 2 ++ config.yaml | 12 +++++---- config_hillclimb.yaml | 30 +++++++++++++++++++++ 6 files changed, 102 insertions(+), 8 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 algorithms/random_search.py create mode 100644 config_hillclimb.yaml diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..b3193ba --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,4 @@ +{ + "python-envs.defaultEnvManager": "ms-python.python:pyenv", + "python-envs.pythonProjects": [] +} diff --git a/algorithms/hill_climb.py b/algorithms/hill_climb.py index 49a3480..fd589f5 100644 --- a/algorithms/hill_climb.py +++ b/algorithms/hill_climb.py @@ -28,7 +28,7 @@ def _scenario_callback( if self.waypoint_index == 0: # if spawn point previouse_index = len(waypoints) - 1 # pick last point else: - previouse_index = self.waypoint_index - 1 + previouse_index = self.waypoint_index - 1 # else pick previous waypoints[previouse_index] = self.prev_waypoints else: self.prev_ds = driving_score @@ -53,9 +53,11 @@ def __find_new_neighbour_point(self, current_point: dict) -> dict: current_point["position"]["y"], ) - all_points = self.__get_all_lanelet_points() + all_points = ( + self.__get_all_lanelet_points() + ) # get all lanelet 2 points (centerline) self.visited_points.add(current_point_) - all_points.difference(self.visited_points) + all_points.difference(self.visited_points) # find unvisited points points_in_radius = [] for point in all_points: diff --git a/algorithms/random_search.py b/algorithms/random_search.py new file mode 100644 index 0000000..047cdad --- /dev/null +++ b/algorithms/random_search.py @@ -0,0 +1,54 @@ +from basic_algorithm import BasicAlgorithm +import lanelet2 + +import numpy as np + + +class RandomSearch(BasicAlgorithm): + def _init__(self, args: dict) -> None: + self._args = args + + self.lanelet2 = args["lanelet2"] + self.seed = args["seed"] + + self.bounds = [args["lower_bound"], args["upper_bound"]] + # initialise the numpy seeded generator + self._rng = np.random.default_rng(self.seed) + + # state + self.prev_ds = 0 + self.all_points = self.__get_all_lanelet_points() # stored in memory + + def _scenario_callback( + self, scenario_definition: dict, driving_score: float + ) -> dict: + valid = False + spawn = None + checkpoint = None + + while not valid: + spawn = self._rng.choice(self.all_points) + checkpoint = self._rng.choice(self.all_points) + + if self.bounds[0] < self._dist(spawn, checkpoint) < self.bounds[1]: + valid = True + + scenario_definition["routes"][0]["route"]["waypoints"] = [spawn, checkpoint] + return scenario_definition + + def _np_to_json(self, p1: np.ndarray) -> dict: + return {"position": {"x": p1[0], "y": p1[1], "z": 0.0}} + + def __get_all_lanelet_points(self) -> np.ndarray: + map = lanelet2.io.load(self.lanelet2, lanelet2.io.Origin(0, 0)) + lanelets = map.laneletLayer + + centerline_points = [] + for lanelet in list(lanelets): + for points in lanelet.centerline: + centerline_points.append(np.asarray([points.x, points.y])) + + return np.asarray(centerline_points) # convert to numpy array + + def _dist(self, p1: np.ndarray, p2: np.ndarray) -> np.floating: + return np.linalg.norm(p1 - p2) diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index 41c577a..4c573e4 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -410,6 +410,8 @@ def _calculate_driving_score(self, criteria: dict) -> float: else: logger.info(f"Condition {infraction}: Found zero breaches") + penalties += delta_penalty + driving_score = completed_route * (1 / penalties) return driving_score diff --git a/config.yaml b/config.yaml index 97ab270..e02f7fa 100644 --- a/config.yaml +++ b/config.yaml @@ -4,7 +4,7 @@ log: log_format: '[%(levelname)s] [%(created)f] [%(filename)s] %(message)s' carla: host: 127.0.0.1 - fidelity: 'Low' + fidelity: 'High' port: 2000 timeout: 20 sync: true @@ -18,13 +18,15 @@ scenario_runner: debug: false dev_mode: false follow_ego: true - json: ./example_scenario.json route_id: 0 + json: ./example_scenario.json agent: srunner/autoagents/autoware_agent initialisation_budget: 200 # ticks algorithm: iterations: 10 - path: algorithms/hill_climb + path: algorithms/random_search args: # will be passed into the algorithm class as a dict - lanelet_path: /autoware_scenario_runner/algorithms/resources/Town01.osm - radius: 10 + lanelet2: /autoware_scenario_runner/algorithms/resources/Town01.osm + seed: 10 + lower_bound: 20 + upper_bound: 150 diff --git a/config_hillclimb.yaml b/config_hillclimb.yaml new file mode 100644 index 0000000..97ab270 --- /dev/null +++ b/config_hillclimb.yaml @@ -0,0 +1,30 @@ +log: + path: 'logs/' + clear_old_logs: true + log_format: '[%(levelname)s] [%(created)f] [%(filename)s] %(message)s' +carla: + host: 127.0.0.1 + fidelity: 'Low' + port: 2000 + timeout: 20 + sync: true + fixed_delta_seconds: 0.05 # update rate 1 / FPS +traffic_manager: + active: true + sync: true # must be the same as carla sync + seed: 0 + port: 8001 +scenario_runner: + debug: false + dev_mode: false + follow_ego: true + json: ./example_scenario.json + route_id: 0 + agent: srunner/autoagents/autoware_agent + initialisation_budget: 200 # ticks + algorithm: + iterations: 10 + path: algorithms/hill_climb + args: # will be passed into the algorithm class as a dict + lanelet_path: /autoware_scenario_runner/algorithms/resources/Town01.osm + radius: 10 From 775c00ac746be4a459f5a4c5861157fc1b754b39 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Wed, 10 Sep 2025 15:40:22 +0100 Subject: [PATCH 3/9] removed VScode settings --- .vscode/settings.json | 4 ---- 1 file changed, 4 deletions(-) delete mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index b3193ba..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,4 +0,0 @@ -{ - "python-envs.defaultEnvManager": "ms-python.python:pyenv", - "python-envs.pythonProjects": [] -} From a8ecc08fa69abcd7fc0ef3767326e0322d51154e Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Wed, 10 Sep 2025 15:47:04 +0100 Subject: [PATCH 4/9] changed actions to run on self-hosted --- .github/workflows/docker-image.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/docker-image.yml b/.github/workflows/docker-image.yml index 92650fd..87b8d94 100644 --- a/.github/workflows/docker-image.yml +++ b/.github/workflows/docker-image.yml @@ -6,7 +6,7 @@ on: jobs: build-and-push: - runs-on: ubuntu-latest + runs-on: self-hosted permissions: contents: read packages: write From ba313026cc5aaab2087f69957d87cf1848f87104 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Wed, 10 Sep 2025 16:01:37 +0100 Subject: [PATCH 5/9] fixed typo with RandomSearch init function, moved MetricsCollector.init_state() call to within process; removed print statements from MetricsCollector thread --- algorithms/random_search.py | 2 +- aw_scenario_runner.py | 18 +++++++++--------- srunner/tools/metrics_collector.py | 5 +---- 3 files changed, 11 insertions(+), 14 deletions(-) diff --git a/algorithms/random_search.py b/algorithms/random_search.py index 047cdad..c0ee282 100644 --- a/algorithms/random_search.py +++ b/algorithms/random_search.py @@ -5,7 +5,7 @@ class RandomSearch(BasicAlgorithm): - def _init__(self, args: dict) -> None: + def __init__(self, args: dict) -> None: self._args = args self.lanelet2 = args["lanelet2"] diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index 4c573e4..faffb32 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -132,6 +132,15 @@ def run_scenario( scenario_name: str, result_, ) -> None: + logger.info("Starting the MetricsCollector thread...") + + MetricsCollector.reset() + MetricsCollector.init_state( + metrics_collected, + os.path.join(self.results_manager.last_scenario, "execution_time.txt"), + include=False, + ) + logger.info("Connecting to client...") self.carla_client = carla.Client( self._carla_config["host"], int(self._carla_config["port"]) @@ -308,15 +317,6 @@ def run(self) -> None: self.results_manager.last_scenario, env_config )[self._scenario_config["route_id"]] - logger.info("Starting the MetricsCollector thread...") - - MetricsCollector.reset() - MetricsCollector.init_state( - metrics_collected, - os.path.join(self.results_manager.last_scenario, "execution_time.txt"), - include=False, - ) - logger.info("Starting scenario in new process...") result_dict = {"status": False, "criteria": {}} diff --git a/srunner/tools/metrics_collector.py b/srunner/tools/metrics_collector.py index 5e23306..8d2d5d5 100644 --- a/srunner/tools/metrics_collector.py +++ b/srunner/tools/metrics_collector.py @@ -125,15 +125,12 @@ def _thread_target(cls) -> None: json.dump(state, f) is_first_item = False - print("Pushed") _pos += 1 if _pos % cls._flush_freq == 0: - print("Flushing buffer") - start = time.perf_counter() f.flush() os.fsync(f.fileno()) - print(f"Flushing took {time.perf_counter() - start}ms") + except Empty: # Queue was empty, just continue the loop to check cls._running again pass From d3e01f4737ffd6eeba620153a06b006fa09cfd31 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Wed, 10 Sep 2025 16:17:11 +0100 Subject: [PATCH 6/9] added Town01 to algorithms/resources, fixed bug with MetricsCollector thread never ending --- algorithms/resources/Town01.osm | 12651 ++++++++++++++++++ aw_scenario_runner.py | 4 +- srunner/scenariomanager/scenario_manager.py | 15 +- 3 files changed, 12663 insertions(+), 7 deletions(-) create mode 100644 algorithms/resources/Town01.osm diff --git a/algorithms/resources/Town01.osm b/algorithms/resources/Town01.osm new file mode 100644 index 0000000..1b4ce84 --- /dev/null +++ b/algorithms/resources/Town01.osm @@ -0,0 +1,12651 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index faffb32..0bcff71 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -134,7 +134,6 @@ def run_scenario( ) -> None: logger.info("Starting the MetricsCollector thread...") - MetricsCollector.reset() MetricsCollector.init_state( metrics_collected, os.path.join(self.results_manager.last_scenario, "execution_time.txt"), @@ -251,6 +250,9 @@ def run_scenario( ) result = False + # stop the MetricsCollector thread + MetricsCollector.reset() + # analyse the scenario criteria = self._output_criteria( self.scenario_manager.scenario.get_criteria(), # type: ignore diff --git a/srunner/scenariomanager/scenario_manager.py b/srunner/scenariomanager/scenario_manager.py index aea99e5..f1a0d77 100644 --- a/srunner/scenariomanager/scenario_manager.py +++ b/srunner/scenariomanager/scenario_manager.py @@ -128,7 +128,7 @@ def run_scenario(self): self._running = True while self._running: - _tick_start = time.perf_counter() + _tick_start = time.perf_counter_ns() / 1e6 timestamp = None world = CarlaDataProvider.get_world() if world: @@ -139,7 +139,9 @@ def run_scenario(self): self._tick_scenario(timestamp) MetricsCollector.update_key("timestamp", _tick_start) - MetricsCollector.update_key("total_tick", time.perf_counter() - _tick_start) + MetricsCollector.update_key( + "total_tick", (time.perf_counter_ns() / 1e6) - _tick_start + ) # calculate latency _state = MetricsCollector.fetch_state() @@ -177,11 +179,11 @@ def _tick_scenario(self, timestamp): if self._debug_mode: print("\n--------- Tick ---------\n") - _tick_carla_start = time.perf_counter() + _tick_carla_start = time.perf_counter_ns() / 1e6 if self._sync_mode and self._watchdog.get_status(): CarlaDataProvider.get_world().tick() MetricsCollector.update_key( - "carla_time", time.perf_counter() - _tick_carla_start + "carla_time", (time.perf_counter_ns() / 1e6) - _tick_carla_start ) # Update game time and actor information @@ -192,10 +194,11 @@ def _tick_scenario(self, timestamp): self._agent() # pylint: disable=not-callable # Tick scenario - _scenario_tick_start = time.perf_counter() + _scenario_tick_start = time.perf_counter_ns() / 1e6 self.scenario_tree.tick_once() MetricsCollector.update_key( - "scenario_runner_time", time.perf_counter() - _scenario_tick_start + "scenario_runner_time", + (time.perf_counter_ns() / 1e6) - _scenario_tick_start, ) if self.follow_ego: From cf709faaacc3e22b60c749ca2fae9ba3d7ad26da Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Wed, 10 Sep 2025 16:44:36 +0100 Subject: [PATCH 7/9] fixed bug with agent shortening route; fixed bug with RandomSearch --- algorithms/random_search.py | 5 ++++- srunner/autoagents/autoware_agent.py | 13 ++++++++----- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/algorithms/random_search.py b/algorithms/random_search.py index c0ee282..38829b3 100644 --- a/algorithms/random_search.py +++ b/algorithms/random_search.py @@ -33,7 +33,10 @@ def _scenario_callback( if self.bounds[0] < self._dist(spawn, checkpoint) < self.bounds[1]: valid = True - scenario_definition["routes"][0]["route"]["waypoints"] = [spawn, checkpoint] + scenario_definition["routes"][0]["route"]["waypoints"] = [ + self._np_to_json(spawn), + self._np_to_json(checkpoint), + ] return scenario_definition def _np_to_json(self, p1: np.ndarray) -> dict: diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index 28b41f0..b206b67 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -100,6 +100,13 @@ def set_route(self) -> None: self.goal_pose_world = self._global_plan_world_coord[-1] self.waypoints_world = self._global_plan_world_coord[:-1] + # autoware cannot handle many waypoints, becomes unreliable + n_waypoints = len(self.waypoints_world) + segment_size = int(n_waypoints / 3) + + if segment_size > 1: + self.waypoints_world = self.waypoints_world[0::segment_size] + logger.info("Clearing route...") self.route_node.request_clear_route() @@ -156,11 +163,7 @@ def run_step_init(self) -> bool: 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.publish_route(goal_pose, waypoints[0::segment_size]) + self.route_node.publish_route(goal_pose, waypoints) self.sent_route = True # check if the current route is set and we are able to send engage From 438a25c16a9d75df488c32e3455e35a09bbfc95c Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Wed, 10 Sep 2025 17:56:50 +0100 Subject: [PATCH 8/9] moved scenario manager initialisation into new process --- aw_scenario_runner.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index 0bcff71..e973156 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -97,11 +97,7 @@ def __init__(self, config: dict) -> None: self.module_algorithm = importlib.import_module(alg_module) # main class to execute scenarios - self.scenario_manager = ScenarioManager( - self.DEBUG, - self._carla_config["sync"], - self._carla_config["timeout"], - ) + self.scenario_manager = None self.results_manager = ScenarioDefinitionManager() @@ -132,6 +128,13 @@ def run_scenario( scenario_name: str, result_, ) -> None: + logger.info("Initialising Scenario Manager...") + self.scenario_manager = ScenarioManager( + self.DEBUG, + self._carla_config["sync"], + self._carla_config["timeout"], + ) + logger.info("Starting the MetricsCollector thread...") MetricsCollector.init_state( From 5f455e893cfa82e2531ce4fda43416a80cd92914 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Thu, 11 Sep 2025 13:23:45 +0100 Subject: [PATCH 9/9] refactored algorithm to be ran in scenario process; fixed RandomSearch being reinitialised with identical seed; added new timing metrics --- algorithms/basic_algorithm.py | 9 ++++ algorithms/random_search.py | 48 +++++++++++++----- aw_scenario_runner.py | 8 ++- config.yaml | 8 ++- docker/autoware_msgs.tar | Bin 645120 -> 645120 bytes example_scenario.json | 2 +- srunner/autoagents/autonomous_agent.py | 8 --- srunner/autoagents/autoware_agent.py | 5 +- .../autoagents/autoware_nodes/tick_node.py | 17 +++++-- srunner/scenariomanager/scenario_manager.py | 4 +- 10 files changed, 73 insertions(+), 36 deletions(-) diff --git a/algorithms/basic_algorithm.py b/algorithms/basic_algorithm.py index bd270b7..2380dde 100644 --- a/algorithms/basic_algorithm.py +++ b/algorithms/basic_algorithm.py @@ -1,7 +1,12 @@ +import numpy as np + + class BasicAlgorithm(object): def __init__(self, args: dict) -> None: self._args = args + self._rng = None + def _scenario_callback( self, scenario_definition: dict, driving_score: float ) -> dict: @@ -11,3 +16,7 @@ def _scenario_callback( """ raise NotImplementedError("This function should be implemented by the user") + + def _update_generator(self, seed: int) -> None: + """Update the random seeded BitGenerator with a new seed""" + self._rng = np.random.default_rng(seed) diff --git a/algorithms/random_search.py b/algorithms/random_search.py index 38829b3..177274e 100644 --- a/algorithms/random_search.py +++ b/algorithms/random_search.py @@ -1,6 +1,8 @@ from basic_algorithm import BasicAlgorithm -import lanelet2 +from srunner.tools import route_manipulation +import lanelet2 +import carla import numpy as np @@ -9,13 +11,6 @@ def __init__(self, args: dict) -> None: self._args = args self.lanelet2 = args["lanelet2"] - self.seed = args["seed"] - - self.bounds = [args["lower_bound"], args["upper_bound"]] - # initialise the numpy seeded generator - self._rng = np.random.default_rng(self.seed) - - # state self.prev_ds = 0 self.all_points = self.__get_all_lanelet_points() # stored in memory @@ -24,27 +19,41 @@ def _scenario_callback( ) -> dict: valid = False spawn = None - checkpoint = None + goalpose = None while not valid: spawn = self._rng.choice(self.all_points) - checkpoint = self._rng.choice(self.all_points) + goalpose = self._rng.choice(self.all_points) - if self.bounds[0] < self._dist(spawn, checkpoint) < self.bounds[1]: - valid = True + valid = self._valid_route([spawn, goalpose]) and self._not_same_lane_check( + spawn, goalpose + ) scenario_definition["routes"][0]["route"]["waypoints"] = [ self._np_to_json(spawn), - self._np_to_json(checkpoint), + self._np_to_json(goalpose), ] return scenario_definition + def _update_generator(self, seed: int) -> None: + self._rng = np.random.default_rng(seed) + + def _to_carla(self, point: np.ndarray) -> carla.Location: + return carla.Location(point[0], point[1], 0.0) + + def _valid_route(self, route) -> bool: + carla_route = list(map(self._to_carla, route)) + + gps_route, route = route_manipulation.interpolate_trajectory(carla_route) + return not ((len(gps_route) == 1) and (len(route) == 1)) + def _np_to_json(self, p1: np.ndarray) -> dict: return {"position": {"x": p1[0], "y": p1[1], "z": 0.0}} def __get_all_lanelet_points(self) -> np.ndarray: map = lanelet2.io.load(self.lanelet2, lanelet2.io.Origin(0, 0)) lanelets = map.laneletLayer + self.lanelet_map = lanelets centerline_points = [] for lanelet in list(lanelets): @@ -53,5 +62,18 @@ def __get_all_lanelet_points(self) -> np.ndarray: return np.asarray(centerline_points) # convert to numpy array + def _not_same_lane_check(self, p1, p2): + lanelets = [ + lanelet2.geometry.findWithin(self.lanelet_map, p1, 0), + lanelet2.geometry.findWithin(self.lanelet_map, p2, 0), + ] + + lanelet_ids = [ + {ll.id for dist, ll in lanelets[0]}, + {ll.id for dist, ll in lanelets[1]}, + ] + common_lanes = lanelet_ids[0].intersection(lanelet_ids[1]) + return common_lanes == 0 # 0 means no shared lanes + def _dist(self, p1: np.ndarray, p2: np.ndarray) -> np.floating: return np.linalg.norm(p1 - p2) diff --git a/aw_scenario_runner.py b/aw_scenario_runner.py index e973156..eb005b0 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -47,7 +47,13 @@ "timestamp": 0.0, # when tick started "total_tick": 0.0, "scenario_runner_time": 0.0, - "agent_time": 0.0, + "agent_time": { + "snapshot": 0.0, + "state": 0.0, + "sensor": 0.0, + "control": 0.0, + "agent_total": 0.0, + }, "latency": 0.0, "carla_time": 0.0, } diff --git a/config.yaml b/config.yaml index e02f7fa..233518e 100644 --- a/config.yaml +++ b/config.yaml @@ -23,10 +23,8 @@ scenario_runner: agent: srunner/autoagents/autoware_agent initialisation_budget: 200 # ticks algorithm: - iterations: 10 + iterations: 50 path: algorithms/random_search - args: # will be passed into the algorithm class as a dict + seed: 10 + args: lanelet2: /autoware_scenario_runner/algorithms/resources/Town01.osm - seed: 10 - lower_bound: 20 - upper_bound: 150 diff --git a/docker/autoware_msgs.tar b/docker/autoware_msgs.tar index 23e6e992f6dd0b09b9a4ec336edb08d741889cc9..270fd671612028222344cf9b1573539177eb7ec2 100644 GIT binary patch delta 204 zcmZqJpx&@SeM5JEkePw0vAMCCF@u4Tk(s$6gM#Vg#C0j#8F#WvGftKX%8)cNHe@g~ zH9%5lW?*E@pkM$}x0z9zk#RCph_6R+USdIUMt%ubT26jqiLsGFaY8ftX{vK^W(SMgYarC0+mk diff --git a/example_scenario.json b/example_scenario.json index 765275d..07135e3 100644 --- a/example_scenario.json +++ b/example_scenario.json @@ -32,7 +32,7 @@ } ], "waypoints": [ - { + { "position": { "x": 88.4, "y": 82.2, diff --git a/srunner/autoagents/autonomous_agent.py b/srunner/autoagents/autonomous_agent.py index 59a1407..e081a44 100644 --- a/srunner/autoagents/autonomous_agent.py +++ b/srunner/autoagents/autonomous_agent.py @@ -86,14 +86,6 @@ def __call__(self): Execute the agent call, e.g. agent() Returns the next vehicle controls """ - # no need for any of this, handled by autoware - # input_data = self.sensor_interface.get_data() - - # timestamp = GameTime.get_time() - # wallclock = GameTime.get_wallclocktime() - # print('======[Agent] Wallclock_time = {} / Sim_time = {}'.format(wallclock, timestamp)) - # control.manual_gear_shift = False - self.run_step() def set_global_plan(self, global_plan_gps, global_plan_world_coord): diff --git a/srunner/autoagents/autoware_agent.py b/srunner/autoagents/autoware_agent.py index b206b67..64f5fbf 100644 --- a/srunner/autoagents/autoware_agent.py +++ b/srunner/autoagents/autoware_agent.py @@ -123,7 +123,7 @@ def _convert_to_waypoint(self, point): node=self.autoware_node, ) - def cleanup(self) -> None: + def destroy(self) -> None: """Cleanup""" logger.info("Sending shutdown signal to autoware...") self.state_node.reset_autoware() @@ -138,6 +138,9 @@ def cleanup(self) -> None: except RuntimeError: logger.info("Failed to clean up executor thread...") + def cleanup(self) -> None: + self.destroy() + def run_step_init(self) -> bool: """Route Initialisation loop diff --git a/srunner/autoagents/autoware_nodes/tick_node.py b/srunner/autoagents/autoware_nodes/tick_node.py index 1986b74..fce5d7c 100644 --- a/srunner/autoagents/autoware_nodes/tick_node.py +++ b/srunner/autoagents/autoware_nodes/tick_node.py @@ -40,12 +40,21 @@ def autoware_tick(self) -> None: if self.debug: self.get_logger().info( - f"Service {self.tick_service} responded with delta of {res.delta}ms" + f"Service {self.tick_service} responded with delta of {res.agent_total}ms" ) - MetricsCollector.update_key("agent_time", res.delta) - - if res.delta == 0.0: + MetricsCollector.update_key( + "agent_time", + { + "snapshot": res.snapshot, + "state": res.state, + "sensor": res.sensor, + "control": res.control, + "agent_total": res.agent_total, + }, + ) + + if res.agent_total == 0.0: self.get_logger().info( f"Service {self.tick_service} responded with invalid time-delta. Is CARLA running?" ) diff --git a/srunner/scenariomanager/scenario_manager.py b/srunner/scenariomanager/scenario_manager.py index f1a0d77..9e439e6 100644 --- a/srunner/scenariomanager/scenario_manager.py +++ b/srunner/scenariomanager/scenario_manager.py @@ -94,8 +94,6 @@ def cleanup(self): self._watchdog.stop() self._watchdog = None - CarlaDataProvider.cleanup() - def load_scenario(self, scenario, agent=None, follow_ego=False): """ Load a new scenario @@ -148,7 +146,7 @@ def run_scenario(self): latency = ( _state["total_tick"] - _state["scenario_runner_time"] - - _state["agent_time"] + - _state["agent_time"]["agent_total"] - _state["carla_time"] )