From 3b2f269d5d7adea8a4678c7082b2435526c2d261 Mon Sep 17 00:00:00 2001 From: Gwilym Rutherford Date: Mon, 28 Jul 2025 10:32:42 +0100 Subject: [PATCH] Add simple annealing and hill climbing algorithm --- algorithms/Simulated_Annealing.py | 106 ++++++++++++++++++++++++++++++ algorithms/hill_climbing.py | 86 ++++++++++++++++++++++++ 2 files changed, 192 insertions(+) create mode 100644 algorithms/Simulated_Annealing.py create mode 100644 algorithms/hill_climbing.py diff --git a/algorithms/Simulated_Annealing.py b/algorithms/Simulated_Annealing.py new file mode 100644 index 0000000..44aa3c3 --- /dev/null +++ b/algorithms/Simulated_Annealing.py @@ -0,0 +1,106 @@ +# Resources used +# https://colab.research.google.com/github/bnsreenu/python_for_microscopists/blob/master/319_what_is_simulated_annealing.ipynb#scrollTo=5SSQpSpHeUZh +# https://en.wikipedia.org/wiki/Simulated_annealing + +from BasicAlgorithm import BasicAlgorithm +import lanelet2 +import random +import math + +from math import sqrt, pow + + +class Simulated_Annealing(BasicAlgorithm): + def __init__( + self, temperature, min_temperature, temperature_step, radius, lanelet_path + ) -> None: + super(BasicAlgorithm).__init__() + self.temperature = temperature + self.min_temperature = min_temperature + self.temperature_step = temperature_step + + self.radius = radius + self.lanelet_path = lanelet_path + + self.prev_ds = None + self.prev_waypoints = None + self.waypoints_index = 0 + + def _scenario_callback( + self, scenario_definition: dict, driving_score: float + ) -> dict: + waypoints = scenario_definition["routes"][0]["waypoints"] + + if self.temperature <= self.min_temperature: + print("Reached min temperature, no more iterations needed") + print("=========final waypoint values are=========") + print(self.prev_waypoints) + print("=========final driving score===============") + print(self.prev_ds) + print("===========================================") + # solution is prev_ds and prev_waypoints + return + + if self.prev_ds is not None: + ds_diff = self.prev_ds - driving_score # we want a worse driving score + ds_diff = driving_score - self.prev_ds + if ds_diff > 0: + self.prev_waypoints = waypoints[self.waypoint_index] + self.prev_ds = driving_score + elif math.exp(ds_diff / self.temperature) > random.randint(0, 1): + self.prev_waypoints = waypoints[self.waypoint_index] + self.prev_ds = driving_score + + self.temperature -= self.temperature_step + else: + self.prev_ds = driving_score + self.prev_waypoints = waypoints[self.waypoint_index] + + new_point = self.__find_new_neighbour_point(waypoints[self.waypoints_index]) + waypoints[self.waypoints_index] = new_point + + if self.waypoint_index + 1 > len(waypoints): + self.waypoint_index = 0 + else: + self.waypoint_index += 1 + + return scenario_definition + + def __find_new_neighbour_point(self, current_point: dict) -> dict: + current_point_ = ( + current_point["position"]["x"], + current_point["position"]["y"], + ) + + all_points = self.__get_all_lanelet_points() + self.visited_points.add(current_point_) + all_points.difference(self.visited_points) + + points_in_radius = [] + for point in all_points: + if ( + self.radius == -1 + or self.__euclidian_distance(point, current_point_) <= self.radius + ): + points_in_radius.append(point) + + if len(points_in_radius) == 0: + random_point = random.choice(list(all_points)) + else: + random_point = random.choice(points_in_radius) + + return {"positions": {"x": random_point[0], "y": random_point[1]}} + + def __get_all_lanelet_points(self) -> set[tuple[int, int]]: + map = lanelet2.io.load(self.lanelet_path, lanelet2.io.Origin(0, 0)) + lanelets = map.laneletLayer + + centerline_points = [] + for lanelet in list(lanelets): + for points in lanelet.centerline: + centerline_points += (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)) diff --git a/algorithms/hill_climbing.py b/algorithms/hill_climbing.py new file mode 100644 index 0000000..e7e9e5b --- /dev/null +++ b/algorithms/hill_climbing.py @@ -0,0 +1,86 @@ +from basic_algorithm import BasicAlgorithm +import lanelet2 +import random + +from math import sqrt, pow + + +class Hill_Climb(BasicAlgorithm): + def __init__(self, radius, lanelet_path) -> None: + super(BasicAlgorithm).__init__() + self.radius = radius + self.lanelet_path = lanelet_path + self.visited_points: set[tuple[int, int]] = set() + + self.prev_ds = None + self.prev_waypoints = None + self.waypoint_index = 0 + + def _scenario_callback( + self, scenario_definition: dict, driving_score: float + ) -> dict: + # pass in route id + waypoints = scenario_definition["routes"][0]["waypoints"] + + if self.prev_ds is not None: + if not driving_score >= self.prev_ds: + if self.waypoint_index == 0: + previouse_index = len(waypoints) + else: + previouse_index = self.waypoint_index - 1 + waypoints[previouse_index] = self.prev_waypoints + else: + self.prev_ds = driving_score + self.prev_waypoints = waypoints[self.waypoint_index] + + new_point = self.__find_new_neighbour_point(waypoints[self.waypoint_index]) + + waypoints[self.waypoint_index] = new_point + + self.prev_waypoints = waypoints[self.waypoint_index] + + if self.waypoint_index + 1 > len(waypoints): + self.waypoint_index = 0 + else: + self.waypoint_index += 1 + + return scenario_definition + + def __find_new_neighbour_point(self, current_point: dict) -> dict: + current_point_ = ( + current_point["position"]["x"], + current_point["position"]["y"], + ) + + all_points = self.__get_all_lanelet_points() + self.visited_points.add(current_point_) + all_points.difference(self.visited_points) + + points_in_radius = [] + for point in all_points: + if ( + self.radius == -1 + or self.__euclidian_distance(point, current_point_) <= self.radius + ): + points_in_radius.append(point) + + if len(points_in_radius) == 0: + random_point = random.choice(list(all_points)) + else: + random_point = random.choice(points_in_radius) + + return {"positions": {"x": random_point[0], "y": random_point[1]}} + + def __get_all_lanelet_points(self) -> set[tuple[int, int]]: + map = lanelet2.io.load(self.lanelet_path, lanelet2.io.Origin(0, 0)) + lanelets = map.laneletLayer + + centerline_points = [] + for lanelet in list(lanelets): + for points in lanelet.centerline: + centerline_points += (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))