From 23a9347672e743f23f90f4d966d698f3f959f772 Mon Sep 17 00:00:00 2001 From: David Gasinski Date: Tue, 16 Sep 2025 11:24:08 +0100 Subject: [PATCH] added seed generator to scenario process --- algorithms/random_search.py | 20 ++++++------- aw_scenario_runner.py | 60 ++++++++++++++++++++++++------------- 2 files changed, 49 insertions(+), 31 deletions(-) diff --git a/algorithms/random_search.py b/algorithms/random_search.py index 177274e..ba7f02a 100644 --- a/algorithms/random_search.py +++ b/algorithms/random_search.py @@ -25,9 +25,7 @@ def _scenario_callback( spawn = self._rng.choice(self.all_points) goalpose = self._rng.choice(self.all_points) - valid = self._valid_route([spawn, goalpose]) and self._not_same_lane_check( - spawn, goalpose - ) + valid = self._valid_route([spawn, goalpose]) scenario_definition["routes"][0]["route"]["waypoints"] = [ self._np_to_json(spawn), @@ -51,21 +49,23 @@ 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 + laneletmap_ = lanelet2.io.load(self.lanelet2, lanelet2.io.Origin(0, 0)) + self.laneletmap_ = laneletmap_ centerline_points = [] - for lanelet in list(lanelets): + for lanelet in list(self.laneletmap_.laneletLayer): for points in lanelet.centerline: centerline_points.append(np.asarray([points.x, points.y])) return np.asarray(centerline_points) # convert to numpy array def _not_same_lane_check(self, p1, p2): + p1 = lanelet2.core.BasicPoint2d(p1[0], p1[1]) + p2 = lanelet2.core.BasicPoint2d(p2[0], p2[1]) + lanelets = [ - lanelet2.geometry.findWithin(self.lanelet_map, p1, 0), - lanelet2.geometry.findWithin(self.lanelet_map, p2, 0), + lanelet2.geometry.findNearest(self.laneletmap_.laneletLayer, p1, 1), + lanelet2.geometry.findNearest(self.laneletmap_.laneletLayer, p2, 1), ] lanelet_ids = [ @@ -73,7 +73,7 @@ def _not_same_lane_check(self, p1, p2): {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 + return not common_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 eb005b0..498b4a8 100644 --- a/aw_scenario_runner.py +++ b/aw_scenario_runner.py @@ -15,6 +15,7 @@ import carla import time +import numpy as np from srunner.scenariomanager.scenario_manager import ScenarioManager from srunner.tools.results_manager import ScenarioDefinitionManager @@ -33,15 +34,8 @@ from srunner.tools.CARLA_manager import CARLAManager -logger = logging.getLogger("scenario-runner") -infractions_dict = { - "OutsideRouteLanesTest": 0.3, - "CollisionTest": 1.0, - "RunningRedLightTest": 0.4, - "RunningStopTest": 0.25, - "AgentBlockedTest": 0.4, -} +logger = logging.getLogger("scenario-runner") metrics_collected = { "timestamp": 0.0, # when tick started @@ -132,6 +126,7 @@ def run_scenario( route_config: RouteScenarioConfiguration, env_config: EnvironmentConfig, scenario_name: str, + seed: int, result_, ) -> None: logger.info("Initialising Scenario Manager...") @@ -212,7 +207,7 @@ def run_scenario( # allow the agent to localise and set the route budget = int(self._scenario_config["initialisation_budget"]) - status = False # completion status + status = False # completion statusz for tick in range(1, budget + 1): self.carla_world.tick() status = route_config.agent.run_step_init() # type: ignore @@ -267,11 +262,22 @@ def run_scenario( self.scenario_manager.scenario.get_criteria(), # type: ignore f"{self.results_manager.last_scenario}/{scenario_name}.json", ) + logger.info("Calculating driving score...") + driving_score = self._calculate_driving_score(criteria) + + # read the scenario definition + if not self.DEV_MODE: + self.algorithm._update_generator(seed) + + definition = self.algorithm._scenario_callback( + self.json_definition, driving_score + ) # update multipprocessing queue result_dict = result_.get() result_dict["status"] = result - result_dict["criteria"] = criteria + result_dict["definition"] = definition + result_dict["driving_score"] = driving_score result_.put(result_dict) def run(self) -> None: @@ -304,6 +310,8 @@ def run(self) -> None: self._scenario_config["algorithm"]["args"] ) + self._rng = np.random.default_rng(self._scenario_config["algorithm"]["seed"]) + for iteration in range(self.iterations): logger.info("Starting CARLA container....") CARLAManager.restart_carla() @@ -334,12 +342,16 @@ def run(self) -> None: scenario_result = multiprocessing.Queue() scenario_result.put(result_dict) + seed = self._rng.integers( + 0, sys.maxsize + ) # generate a random seed from the seed scenario_process = multiprocessing.Process( target=self.run_scenario, # need to catch connection exception args=( route_config, env_config, scenario_name, + seed, scenario_result, ), ) @@ -359,21 +371,19 @@ def run(self) -> None: "/home/carla/recording.log", self.results_manager.last_scenario, ) + # clean up - delete XML files + self.results_manager.cleanup_xml() + + status = result["status"] + driving_score = result["driving_score"] + self.json_definition = result["definition"] - 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}" ) - - # clean up - delete XML files - self.results_manager.cleanup_xml() - - # read the scenario definition - if not self.DEV_MODE: - self.json_definition = self.algorithm._scenario_callback( - self.json_definition, driving_score - ) + logger.info( + f"Scenario iteration {iteration} ended with status {'SUCCESS' if status else 'FAILURE'}" + ) def _output_criteria( self, criteria, file_name: str, save_file: bool = True @@ -403,6 +413,14 @@ def _output_criteria( return criteria_dict def _calculate_driving_score(self, criteria: dict) -> float: + infractions_dict = { + "OutsideRouteLanesTest": 0.3, + "CollisionTest": 1.0, + "RunningRedLightTest": 0.4, + "RunningStopTest": 0.25, + "AgentBlockedTest": 0.4, + } + driving_score = 0.0 completed_route = float(criteria["RouteCompletionTest"]["actual_value"]) / 100