diff --git a/rl_studio/README.md b/rl_studio/README.md index 713e5867b..de79a49e1 100644 --- a/rl_studio/README.md +++ b/rl_studio/README.md @@ -42,7 +42,8 @@ There are several config files to take as example. If you need more information The following graph shows a conceptual diagram of the operation of RL-Studio in training mode. In the case of making inference or retraining, the process is similar -![](../rl_studio/docs/rls-diagram.svg) +![](../rl_studio/docs/rls-diagram.png) + # Run RL Studio @@ -92,6 +93,7 @@ Remaining params should be adjusted too. There are many working yaml files in co > need extend the rl-studio.py to listen for inputs in a port and execute the loaded brain/algorithm to provide > outputs in the desired way. + More info about how to config and launch any task, please go to [agents](agents/README.md) section. @@ -109,8 +111,3 @@ Launch it with `python plot_waypoints.py -t ` where `` could be Town ![](../rl_studio/docs/Town01_topology.png) - - - - - diff --git a/rl_studio/agents/__init__.py b/rl_studio/agents/__init__.py index c9d7b6430..51dc109e2 100644 --- a/rl_studio/agents/__init__.py +++ b/rl_studio/agents/__init__.py @@ -9,7 +9,6 @@ class TrainerFactory: def __new__(cls, config): - """ There are many options: @@ -53,10 +52,59 @@ def __new__(cls, config): agent=agent, framework=framework, ) + + # ============================= + # Follow Lane - AutoCarla - Carla - Stable-baselines3 + # ============================= + if ( + task == TasksType.FOLLOWLANECARLA.value + and agent == AgentsType.AUTOCARLA.value + # and algorithm == AlgorithmsType.DQN.value + and simulator == EnvsType.CARLA.value + and framework == FrameworksType.STABLE_BASELINES3.value + ): + from rl_studio.agents.auto_carla.train_followlane_carla_sb3 import ( + TrainerFollowLaneAutoCarlaSB3, + ) + + return TrainerFollowLaneAutoCarlaSB3(config) + # ============================= - # FollowLane - F1 - qlearn - Carla + # Follow Lane - AutoCarla - DQN - Carla - TF # ============================= if ( + task == TasksType.FOLLOWLANECARLA.value + and agent == AgentsType.AUTOCARLA.value + and algorithm == AlgorithmsType.DQN.value + and simulator == EnvsType.CARLA.value + and framework == FrameworksType.TF.value + ): + from rl_studio.agents.auto_carla.train_followlane_dqn_carla_tf import ( + TrainerFollowLaneDQNAutoCarlaTF, + ) + + return TrainerFollowLaneDQNAutoCarlaTF(config) + + # ============================= + # Follow Lane - AutoCarla - DDPG - Carla - TF + # ============================= + elif ( + task == TasksType.FOLLOWLANECARLA.value + and agent == AgentsType.AUTOCARLA.value + and algorithm == AlgorithmsType.DDPG.value + and simulator == EnvsType.CARLA.value + and framework == FrameworksType.TF.value + ): + from rl_studio.agents.auto_carla.train_followlane_ddpg_carla_tf import ( + TrainerFollowLaneDDPGAutoCarlaTF, + ) + + return TrainerFollowLaneDDPGAutoCarlaTF(config) + + # ============================= + # FollowLane - AutoCarla - qlearn - Carla + # ============================= + elif ( task == TasksType.FOLLOWLANECARLA.value and agent == AgentsType.AUTOCARLA.value and algorithm == AlgorithmsType.QLEARN.value @@ -71,7 +119,7 @@ def __new__(cls, config): # ============================= # FollowLine - F1 - qlearn - Gazebo # ============================= - if ( + elif ( task == TasksType.FOLLOWLINEGAZEBO.value and agent == AgentsType.F1GAZEBO.value and algorithm == AlgorithmsType.QLEARN.value @@ -340,7 +388,6 @@ def __new__(cls, config): class InferencerFactory: def __new__(cls, config): - agent = config["settings"]["agent"] algorithm = config["settings"]["algorithm"] task = config["settings"].get("task") @@ -355,6 +402,37 @@ def __new__(cls, config): framework=framework, ) + # ============================= + # Follow Lane - AutoCarla - DDPG - Carla - TF + # ============================= + if ( + task == TasksType.FOLLOWLANECARLA.value + and agent == AgentsType.AUTOCARLA.value + and algorithm == AlgorithmsType.DDPG.value + and simulator == EnvsType.CARLA.value + and framework == FrameworksType.TF.value + ): + from rl_studio.agents.auto_carla.inference_followlane_ddpg_carla_tf import ( + InferencerFollowLaneDDPGAutoCarlaTF, + ) + + return InferencerFollowLaneDDPGAutoCarlaTF(config) + + # ============================= + # FollowLane - AutoCarla - qlearn - Carla + # ============================= + if ( + task == TasksType.FOLLOWLANECARLA.value + and agent == AgentsType.AUTOCARLA.value + and algorithm == AlgorithmsType.QLEARN.value + and simulator == EnvsType.CARLA.value + ): + from rl_studio.agents.auto_carla.inference_followlane_qlearn_carla import ( + InferencerFollowLaneQlearnAutoCarla, + ) + + return InferencerFollowLaneQlearnAutoCarla(config) + # ============================= # FollowLine - F1 - qlearn - Gazebo # ============================= diff --git a/rl_studio/agents/auto_carla/actors_sensors.py b/rl_studio/agents/auto_carla/actors_sensors.py new file mode 100644 index 000000000..0724b509f --- /dev/null +++ b/rl_studio/agents/auto_carla/actors_sensors.py @@ -0,0 +1,421 @@ +import math +import weakref + +import carla +import cv2 +import numpy as np +import torch + + +class NewCar(object): + def __init__(self, parent_actor, init_positions, init=None): + self.car = None + self._parent = parent_actor + self.world = self._parent + vehicle = self.world.get_blueprint_library().filter("vehicle.*")[0] + + if init is None: + pose_init = np.random.randint(0, high=len(init_positions)) + else: + pose_init = init + + location = carla.Transform( + carla.Location( + x=init_positions[pose_init][0], + y=init_positions[pose_init][1], + z=init_positions[pose_init][2], + ), + carla.Rotation( + pitch=init_positions[pose_init][3], + yaw=init_positions[pose_init][4], + roll=init_positions[pose_init][5], + ), + ) + + self.car = self.world.spawn_actor(vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(vehicle, location) + + +# ============================================================================== +# -- class RGB CameraManager ------------------------------------------------------------- +# ============================================================================== +class CameraRGBSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.front_rgb_camera = None + + self.world = self._parent.get_world() + bp = self.world.get_blueprint_library().find("sensor.camera.rgb") + bp.set_attribute("image_size_x", f"640") + bp.set_attribute("image_size_y", f"480") + bp.set_attribute("fov", f"110") + + self.sensor = self.world.spawn_actor( + bp, + carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)), + attach_to=self._parent, + ) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraRGBSensor._rgb_image(weak_self, image)) + + @staticmethod + def _rgb_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + image = np.array(image.raw_data) + image = image.reshape((480, 640, 4)) + # image = image.reshape((512, 1024, 4)) + image = image[:, :, :3] + # self._data_dict["image"] = image3 + self.front_rgb_camera = image + + +# ============================================================================== +# -- class Collision sensor ------------------------------------------------------------- +# ============================================================================== +class CollisionSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + # self.collision = None + + self.world = self._parent.get_world() + bp = self.world.get_blueprint_library().find("sensor.other.collision") + # bp.set_attribute("image_size_x", f"640") + # bp.set_attribute("image_size_y", f"480") + # bp.set_attribute("fov", f"110") + + self.sensor = self.world.spawn_actor( + bp, + carla.Transform(carla.Location(x=2.5, z=0.7), carla.Rotation(yaw=+00)), + attach_to=self._parent, + ) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda event: CollisionSensor._collision_data(weak_self, event) + ) + + @staticmethod + def _collision_data(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + + actor_we_collide_against = event.other_actor + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + print( + f"you have crashed with {actor_we_collide_against.type_id} with impulse {impulse} and intensity {intensity}" + ) + + self.collision_hist.append(event) + + +# ============================================================================== +# -- class LaneInvasio sensor ------------------------------------------------------------- +# ============================================================================== +class LaneInvasionSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + # self.collision = None + + self.world = self._parent.get_world() + bp = self.world.get_blueprint_library().find("sensor.other.lane_invasion") + # bp.set_attribute("image_size_x", f"640") + # bp.set_attribute("image_size_y", f"480") + # bp.set_attribute("fov", f"110") + + self.sensor = self.world.spawn_actor( + bp, + carla.Transform(carla.Location(x=2.5, z=0.7), carla.Rotation(yaw=+00)), + attach_to=self._parent, + ) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda event: LaneInvasionSensor._lane_changing(weak_self, event) + ) + + @staticmethod + def _lane_changing(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + + self.lane_changing_hist.append(event) + + +# ============================================================================== +# -- class Obstacle sensor ------------------------------------------------------------- +# ============================================================================== +class ObstacleSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + # self.collision = None + + self.world = self._parent.get_world() + bp = self.world.get_blueprint_library().find("sensor.other.obstacle") + + self.sensor = self.world.spawn_actor( + bp, + carla.Transform(carla.Location(x=2.5, z=0.7), carla.Rotation(yaw=+00)), + attach_to=self._parent, + ) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda event: ObstacleSensor._obstacle_sensor(weak_self, event) + ) + + @staticmethod + def _obstacle_sensor(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + + self.obstacle_hist.append(event) + + +# ============================================================================== +# -- Red Mask CameraManager ------------------------------------------------------------- +# ============================================================================== +class CameraRedMaskSemanticSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.front_red_mask_camera = None + + self.world = self._parent.get_world() + bp = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + bp.set_attribute("image_size_x", f"640") + bp.set_attribute("image_size_y", f"480") + bp.set_attribute("fov", f"110") + + self.sensor = self.world.spawn_actor( + bp, + carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)), + attach_to=self._parent, + ) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda image: CameraRedMaskSemanticSensor._red_mask_semantic_image_callback( + weak_self, image + ) + ) + + @staticmethod + def _red_mask_semantic_image_callback(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + hsv_nemo = cv2.cvtColor(array, cv2.COLOR_RGB2HSV) + + if ( + self.world.get_map().name == "Carla/Maps/Town07" + or self.world.get_map().name == "Carla/Maps/Town04" + or self.world.get_map().name == "Carla/Maps/Town07_Opt" + or self.world.get_map().name == "Carla/Maps/Town04_Opt" + ): + light_sidewalk = (42, 200, 233) + dark_sidewalk = (44, 202, 235) + else: + light_sidewalk = (151, 217, 243) + dark_sidewalk = (153, 219, 245) + + light_pavement = (149, 127, 127) + dark_pavement = (151, 129, 129) + + mask_sidewalk = cv2.inRange(hsv_nemo, light_sidewalk, dark_sidewalk) + # result_sidewalk = cv2.bitwise_and(array, array, mask=mask_sidewalk) + + mask_pavement = cv2.inRange(hsv_nemo, light_pavement, dark_pavement) + # result_pavement = cv2.bitwise_and(array, array, mask=mask_pavement) + + # Adjust according to your adjacency requirement. + kernel = np.ones((3, 3), dtype=np.uint8) + + # Dilating masks to expand boundary. + color1_mask = cv2.dilate(mask_sidewalk, kernel, iterations=1) + color2_mask = cv2.dilate(mask_pavement, kernel, iterations=1) + + # Required points now will have both color's mask val as 255. + common = cv2.bitwise_and(color1_mask, color2_mask) + SOME_THRESHOLD = 0 + + # Common is binary np.uint8 image, min = 0, max = 255. + # SOME_THRESHOLD can be anything within the above range. (not needed though) + # Extract/Use it in whatever way you want it. + intersection_points = np.where(common > SOME_THRESHOLD) + + # Say you want these points in a list form, then you can do this. + pts_list = [[r, c] for r, c in zip(*intersection_points)] + # print(pts_list) + + # for x, y in pts_list: + # image_2[x][y] = (255, 0, 0) + + # red_line_mask = np.zeros((400, 500, 3), dtype=np.uint8) + red_line_mask = np.zeros((image.height, image.width, 3), dtype=np.uint8) + + for x, y in pts_list: + red_line_mask[x][y] = (255, 0, 0) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + red_line_mask = cv2.cvtColor(red_line_mask, cv2.COLOR_BGR2RGB) + self.front_red_mask_camera = red_line_mask + + # AutoCarlaUtils.show_image( + # "states", + # self.front_red_mask_camera, + # 600, + # 400, + # ) + # if self.front_red_mask_camera is not None: + # time.sleep(0.01) + # print(f"self.front_red_mask_camera leyendo") + # print(f"in _red_mask_semantic_image_callback() {time.time()}") + + +# ============================================================================== +# -- class LaneDetector ------------------------------------------------------------- +# ============================================================================== +class LaneDetector: + def __init__(self, model_path: str): + torch.cuda.empty_cache() + self.__model: torch.nn.Module = torch.load(model_path) + self.__model.eval() + self.__threshold = 0.1 + self.__kernel = np.ones((7, 7), np.uint8) + self.__iterations = 4 + + def detect(self, img_array: np.array) -> tuple: + with torch.no_grad(): + image_tensor = img_array.transpose(2, 0, 1).astype("float32") / 255 + x_tensor = torch.from_numpy(image_tensor).to("cuda").unsqueeze(0) + back, left, right = ( + torch.softmax(self.__model.forward(x_tensor), dim=1).cpu().numpy()[0] + ) + + res, left_mask, right_mask = self._lane_detection_overlay( + img_array, left, right + ) + + return res, left_mask, right_mask + + def _lane_detection_overlay( + self, image: np.ndarray, left_mask: np.ndarray, right_mask: np.ndarray + ) -> tuple: + """ + @type left_mask: object + """ + res = np.copy(image) + + cv2.erode(left_mask, self.__kernel, self.__iterations) + cv2.dilate(left_mask, self.__kernel, self.__iterations) + + cv2.erode(left_mask, self.__kernel, self.__iterations) + cv2.dilate(left_mask, self.__kernel, self.__iterations) + + left_mask = self._image_polyfit(left_mask) + right_mask = self._image_polyfit(right_mask) + + # We show only points with probability higher than 0.07 + # show lines in BLUE + # res[left_mask > self.__threshold, :] = [255, 0, 0] # [255, 0, 0] + # res[right_mask > self.__threshold, :] = [255, 0, 0] # [0, 0, 255] + + # show lines in RED + res[left_mask > self.__threshold, :] = [ + 0, + 0, + 255, + ] # [0, 0, 255] # [255, 0, 0] + res[right_mask > self.__threshold, :] = [ + 0, + 0, + 255, + ] # [0, 0, 255] # [0, 0, 255] + return res, left_mask, right_mask + + def _image_polyfit(self, image: np.ndarray) -> np.ndarray: + img = np.copy(image) + img[image > self.__threshold] = 255 + + indices = np.where(img == 255) + + if len(indices[0]) == 0: + return img + grade = 1 + coefficients = np.polyfit(indices[0], indices[1], grade) + + x = np.linspace(0, img.shape[1], num=2500) + y = np.polyval(coefficients, x) + points = np.column_stack((x, y)).astype(int) + + valid_points = [] + + for point in points: + # if (0 < point[1] < 1023) and (0 < point[0] < 509): + if (0 < point[1] < image.shape[1]) and (0 < point[0] < image.shape[0]): + valid_points.append(point) + + valid_points = np.array(valid_points) + polyfitted = np.zeros_like(img) + polyfitted[tuple(valid_points.T)] = 255 + + return polyfitted + + def get_prediction(self, img_array): + """ + original version, not using it + """ + with torch.no_grad(): + image_tensor = img_array.transpose(2, 0, 1).astype("float32") / 255 + x_tensor = torch.from_numpy(image_tensor).to("cuda").unsqueeze(0) + model_output = ( + torch.softmax(self.__model.forward(x_tensor), dim=1).cpu().numpy() + ) + return model_output + + def lane_detection_overlay(self, image, left_mask, right_mask): + """ + original version, not using it + """ + res = np.copy(image) + # We show only points with probability higher than 0.5 + res[left_mask > 0.07, :] = [255, 0, 0] + res[right_mask > 0.07, :] = [0, 0, 255] + + cv2.erode(left_mask, (7, 7), 4) + cv2.dilate(left_mask, (7, 7), 4) + + return res diff --git a/rl_studio/agents/auto_carla/carla_env.py b/rl_studio/agents/auto_carla/carla_env.py new file mode 100644 index 000000000..1291a4425 --- /dev/null +++ b/rl_studio/agents/auto_carla/carla_env.py @@ -0,0 +1,1101 @@ +import math +import random +import time + +import carla +import cv2 +import gymnasium as gym +from gymnasium import spaces +import numpy as np +from sklearn.preprocessing import StandardScaler, MinMaxScaler +import torch + +from rl_studio.agents.auto_carla.actors_sensors import ( + LaneDetector, + NewCar, + CameraRGBSensor, + CameraRedMaskSemanticSensor, + CollisionSensor, + LaneInvasionSensor, + ObstacleSensor, +) +from rl_studio.agents.auto_carla.utils import AutoCarlaUtils +from rl_studio.agents.auto_carla.settings import FollowLaneCarlaConfig + + +correct_normalized_distance = { # based in an image of 640 pixels width + 20: -0.07, + 30: -0.1, + 40: -0.13, + 50: -0.17, + 60: -0.2, + 70: -0.23, + 80: -0.26, + 90: -0.3, + 100: -0.33, + 110: -0.36, + 120: -0.4, + 130: -0.42, + 140: -0.46, + 150: -0.49, + 160: -0.52, + 170: -0.56, + 180: -0.59, + 190: -0.62, + 200: -0.65, + 210: -0.69, + 220: -0.72, +} +correct_pixel_distance = { # based in an image of 640 pixels width + 20: 343, + 30: 353, + 40: 363, + 50: 374, + 60: 384, + 70: 394, + 80: 404, + 90: 415, + 100: 425, + 110: 436, + 120: 446, + 130: 456, + 140: 467, + 150: 477, + 160: 488, + 170: 498, + 180: 508, + 190: 518, + 200: 528, + 210: 540, + 220: 550, +} + + +# ============================================================================== +# -- class CarlaEnv ------------------------------------------------------------- +# ============================================================================== + + +class CarlaEnv(gym.Env): + # def __init__(self, car, sensor_camera_rgb, sensor_camera_lanedetector, config): + # def __init__(self, car, sensor_camera_rgb, sensor_camera_red_mask, config): + def __init__(self, client, world, config): + super(CarlaEnv, self).__init__() + ## --------------- init env + # FollowLaneEnv.__init__(self, **config) + ## --------------- init class variables + FollowLaneCarlaConfig.__init__(self, **config) + + # print(f"{config =}") + + self.image_raw_from_topic = None + self.image_camera = None + # self.sensor = config["sensor"] + + # Image + self.image_resizing = config["image_resizing"] / 100 + self.new_image_size = config["new_image_size"] + self.raw_image = config["raw_image"] + self.height = int(config["height_image"] * self.image_resizing) + self.width = int(config["width_image"] * self.image_resizing) + self.center_image = int(config["center_image"] * self.image_resizing) + self.num_regions = config["num_regions"] + self.pixel_region = int(self.center_image / self.num_regions) * 2 + # self.telemetry_mask = config["telemetry_mask"] + self.poi = config["x_row"][0] + self.image_center = None + self.right_lane_center_image = config["center_image"] + ( + config["center_image"] // 2 + ) + self.lower_limit = config["lower_limit"] + + # States + self.state_space = config["states"] + self.states_entry = config["states_entry"] + if self.state_space == "spn": + self.x_row = [i for i in range(1, int(self.height / 2) - 1)] + else: + self.x_row = config["x_row"] + + # Actions + self.actions_space = config["action_space"] + self.actions = config["actions"] + + # Rewards + self.reward_function = config["reward_function"] + self.rewards = config["rewards"] + self.min_reward = config["min_reward"] + + # Pose + self.town = config["town"] + self.random_pose = config["random_pose"] + self.alternate_pose = config["alternate_pose"] + self.init_pose_number = config["init_pose_number"] + self.finish_pose_number = config["finish_pose_number"] + self.start_alternate_pose = config["start_alternate_pose"] + self.finish_alternate_pose = config["finish_alternate_pose"] + + # print(f"{self.alternate_pose =}\n") + # print(f"{self.start_alternate_pose =}\n") + + self.waypoints_meters = config["waypoints_meters"] + self.waypoints_init = config["waypoints_init"] + self.waypoints_target = config["waypoints_target"] + self.waypoints_lane_id = config["waypoints_lane_id"] + self.waypoints_road_id = config["waypoints_road_id"] + self.max_target_waypoint_distance = config["max_target_waypoint_distance"] + + ############################################################### + # + # gym/stable-baselines3 interface + ############################################################### + + ######## Actions Gym based + # print(f"{len(self.actions) =}") + # print(f"{type(self.actions) =}") + # print(f"{self.actions =}") + # Discrete Actions + if self.actions_space == "carla_discrete": + self.action_space = spaces.Discrete(len(self.actions)) + else: # Continuous Actions + actions_to_array = np.array( + [list(self.actions["v"]), list(self.actions["w"])] + ) + print(f"{actions_to_array =}") + print(f"{actions_to_array[0] =}") + print(f"{actions_to_array[1] =}") + self.action_space = spaces.Box( + low=actions_to_array[0], + high=actions_to_array[1], + shape=(2,), + dtype=np.float32, + ) + print(f"{self.action_space.low = }") + print(f"{self.action_space.high = }") + print(f"{self.action_space.low[0] = }") + print(f"{self.action_space.high[0] = }") + + ######## observations Gym based + # image + if self.state_space == "image": + self.observation_space = spaces.Box( + low=0, high=255, shape=(self.height, self.width, 3), dtype=np.uint8 + ) + else: # Discrete observations vector = [0.0, 3.6, 8.9] + # TODO: change x_row for other list + self.observation_space = spaces.Discrete(len(self.x_row)) # temporary + + # print(f"\n\t In case of implementing Stable-Baselines-gym-based:") + # print( + # f"\n\tIn CarlaEnv --> {self.state_space =}, {self.actions_space =}, {self.action_space =}, {self.observation_space =}" + # ) + + ######################################################################### + + # self.params = config + # print(f"{self.params =}\n") + + self.client = client + self.world = world + + self.new_car = None + self.sensor_camera_rgb = None + self.sensor_camera_red_mask = None + self.sensor_collision = None + self.sensor_lane_invasion = None + self.sensor_obstacle = None + # self.sensor_camera_lanedetector = sensor_camera_lanedetector + self.params = {} + self.is_finish = None + self.dist_to_finish = None + + self.right_line_in_pixels = None + self.lane_centers_in_pixels = None + self.ground_truth_pixel_values = None + self.dist_normalized = None + self.state = None + self.state_right_lines = None + self.drawing_lines_states = [] + self.drawing_numbers_states = [] + + self.lane_changing_hist = [] + self.target_veloc = config["target_vel"] + self.angle = None + self.centers_normal = [] + + self.actor_list = [] + self.collision_hist = [] + self.lane_changing_hist = [] + self.obstacle_hist = [] + + self.spectator = self.world.get_spectator() + ###################################################################### + + ## LaneDetector Camera --------------- + self.sensor_camera_lanedetector = None + # self.sensor_camera_lanedetector = LaneDetector( + # "models/fastai_torch_lane_detector_model.pth" + # ) + + # torch.cuda.empty_cache() + # self.model = torch.load("models/fastai_torch_lane_detector_model.pth") + # self.model.eval() + + def destroy_all_actors_apply_batch(self): + print("\ndestroying %d actors with apply_batch method" % len(self.actor_list)) + self.client.apply_batch( + [carla.command.DestroyActor(x) for x in self.actor_list[::-1]] + ) + + def destroy_all_actors(self): + print("\ndestroying %d actors with destroy() method" % len(self.actor_list)) + for actor in self.actor_list[::-1]: + actor.destroy() + self.actor_list = [] + + def concatenate_states(self, state, state_space): + + N_CONCATENATES = 1 + + if not hasattr(self, "state_queue"): + self.state_queue = [state] * N_CONCATENATES + else: + self.state_queue.pop(0) + self.state_queue.append(state) + + if state_space == "image": + state = np.concatenate(self.state_queue, axis=-1) + else: + state = np.concatenate(self.state_queue) + + return state + + def setup_spectator(self): + # self.spectator = self.world.get_spectator() + car_transfor = self.new_car.car.get_transform() + # world_snapshot = self.world.wait_for_tick() + # actor_snapshot = world_snapshot.find(self.car.id) + # Set spectator at given transform (vehicle transform) + # self.spectator.set_transform(actor_snapshot.get_transform()) + self.spectator.set_transform( + carla.Transform( + car_transfor.location + carla.Location(z=60), + carla.Rotation(pitch=-90, roll=-90), + ) + ) + # self.actor_list.append(self.spectator) + + ##################################################################################### + # + # RESET + # + ##################################################################################### + def reset(self, seed=None, options=None): + """ + In Reset() and step(): + IMPLEMENTING STATES FOR SIMPLIFIED PERCEPTION + STATES = V + W + ANGLE + CENTERS + LINE BORDERS (+ POSE_X + POSE_Y OPTIONALS) + NUMBER OF PARAMETERS = X_ROW * 3 + 3 WHICH IS THE INPUT_SIZE IN NN + FOR RESET() : + V = 0 + W = 0 + ANGLE = 0 + CENTERS = STATES IN PIXELS (DEPENDS ON X_ROW) + LINE BORDERS = + POSE_X, POSE_Y = SPAWNING CAR POINTS IN THE MAP (IT IS A POSSIBILITY, NOT IMPLEMENTED RIGHT NOW) + + EXAMPLE -> states = [V=0, W=0, ANGLE=0, CENTERS=300, 320, 350, LINE_BORDERS = 200, 180, 150, 350, 400, 500] + 12 VALUES WHEN X_ROW = 3 + + """ + self.world.tick() + + if self.sensor_camera_rgb is not None: + self.destroy_all_actors_apply_batch() + + self.sensor_camera_rgb = None + self.sensor_camera_red_mask = None + self.sensor_collision = None + self.sensor_lane_invasion = None + self.sensor_obstacle = None + + # self.lane_sensor = None + # self.obstacle_sensor = None + self.collision_hist = [] + self.lane_changing_hist = [] + self.obstacle_hist = [] + self.actor_list = [] + + ############################################ + ## --------------- Sensors --------------- + ############################################ + + self.sensor_camera_lanedetector = LaneDetector( + "models/fastai_torch_lane_detector_model.pth" + ) + + self.new_car = NewCar( + self.world, + self.start_alternate_pose, + self.alternate_pose, + ) + self.actor_list.append(self.new_car.car) + + ## RGB camera --------------- + self.sensor_camera_rgb = CameraRGBSensor(self.new_car.car) + self.actor_list.append(self.sensor_camera_rgb.sensor) + + ## RedMask Camera --------------- + self.sensor_camera_red_mask = CameraRedMaskSemanticSensor(self.new_car.car) + self.actor_list.append(self.sensor_camera_red_mask.sensor) + + ## Collision --------------- + self.sensor_collision = CollisionSensor(self.new_car.car) + self.actor_list.append(self.sensor_collision.sensor) + + ## Lane Invasion sensor ----------- + self.sensor_lane_invasion = LaneInvasionSensor(self.new_car.car) + self.actor_list.append(self.sensor_lane_invasion.sensor) + + ## Obstacle sensor ---------------- + self.sensor_obstacle = ObstacleSensor(self.new_car.car) + self.actor_list.append(self.sensor_obstacle.sensor) + + self.world.tick() + + ################################################################### + + while self.sensor_camera_rgb.front_rgb_camera is None: + print( + f"\n\treset() ----> {self.sensor_camera_rgb.front_rgb_camera = } ---> waiting for image" + ) + time.sleep(0.2) + + # TODO: spawning car + + self.setup_spectator() + + #### LANEDETECTOR + REGRESSION + image_copy = np.copy(self.sensor_camera_rgb.front_rgb_camera) + ( + image_rgb_lanedetector_regression, + _, + _, + ) = self.sensor_camera_lanedetector.detect(image_copy) + + mask = self.preprocess_image_lane_detector(image_rgb_lanedetector_regression) + + ( + lane_centers_in_pixels, + errors, + errors_normalized, + index_right, + index_left, + ) = self.calculate_lane_centers_with_lane_detector(mask) + + ### CALCULATING STATES AS [lane_centers_in_pixels, index_right, index_left, V, W, ANGLE] + + if self.state_space != "image": + self.state = self.DQN_states_simplified_perception_normalized( + 0, + self.target_veloc, + 0, + 0, + lane_centers_in_pixels, + index_right, + index_left, + mask.shape[1], + ) + else: # IF STATE = image then resize it 32x32, 11x11... + mask = self.preprocess_image_lane_detector( + image_rgb_lanedetector_regression, is_image=True + ) + # input(f"{mask.shape=}") + mask = cv2.resize( + mask, (self.new_image_size, self.new_image_size), cv2.INTER_AREA + ) + # input(f"{mask.shape=}") + + self.state = np.array(np.expand_dims(mask, axis=2)) + + # input(f"{self.state.shape =}") + + self.state = self.concatenate_states(self.state, self.state_space) + # input(f"2 {self.state.shape =}") + # input( + # f"\n\t{self.state_queue =}\n\t{len(self.state_queue) =}\n\t{self.state =}\n\t{len(self.state) =}" + # ) + + return self.state # , states_size + + ##################################################################################### + # + # STEP + # + ##################################################################################### + + def step(self, action): + + lane_detector_pytorch = True + if lane_detector_pytorch: + return self.step_lane_detector_pytorch(action) + + else: + return self.step_only_right_line(action) + + def step_lane_detector_pytorch(self, action): + """ + state: sp + actions: discretes + LaneDetector + """ + + self.control_discrete_actions(action) + + ####################################### LANEDETECTOR + REGRESSION + image_copy = np.copy(self.sensor_camera_rgb.front_rgb_camera) + ( + image_rgb_lanedetector_regression, + left_mask, + right_mask, + ) = self.sensor_camera_lanedetector.detect(image_copy) + + mask = self.preprocess_image_lane_detector(image_rgb_lanedetector_regression) + AutoCarlaUtils.show_image( + "mask", + # mask, + cv2.resize(mask, (400, 200), cv2.INTER_AREA), + 10, + 10, + ) + AutoCarlaUtils.show_image( + "RGB", + # image_copy, + cv2.resize(image_copy, (400, 200), cv2.INTER_AREA), + 1500, + 10, + ) + AutoCarlaUtils.show_image( + "LaneDetector", + # image_rgb_lanedetector_regression[ + # (image_rgb_lanedetector_regression.shape[0] // 2) : + # ], + cv2.resize( + image_rgb_lanedetector_regression[ + (image_rgb_lanedetector_regression.shape[0] // 2) : + ], + (400, 200), + cv2.INTER_AREA, + ), + 1100, + 10, + ) + ( + lane_centers_in_pixels, + errors, + self.centers_normal, + index_right, + index_left, + ) = self.calculate_lane_centers_with_lane_detector(mask) + + # print( + # f"\n\tin step()...\n\t{lane_centers_in_pixels = }\n\t{errors =}\n\t{errors_normalized =}\n\t{index_right = }\n\t{index_left =}" + # ) + + ( + self.states, + drawing_lines_states, + drawing_numbers_states, + ) = self.calculate_states( + mask.shape[1], + lane_centers_in_pixels, + self.num_regions, + size_lateral_states=140, + ) + # print( + # f"\n\t{self.states = }\n\t{drawing_lines_states =}\n\t{drawing_numbers_states =}" + # f"\n\t{self.states = }" + # ) + AutoCarlaUtils.show_image_lines_centers_borders( + "Front Camera", + self.sensor_camera_rgb.front_rgb_camera[ + (self.sensor_camera_rgb.front_rgb_camera.shape[0] // 2) : + ], + self.x_row, + 450, + 10, + index_right, + index_left, + lane_centers_in_pixels, + drawing_lines_states, + drawing_numbers_states, + ) + + ################# Heading + angle_height_image = self.sensor_camera_rgb.front_rgb_camera.shape[0] // 2 + angle_weight_image = self.sensor_camera_rgb.front_rgb_camera.shape[1] // 2 + central_line_points = [ + (angle_weight_image, 0), + [angle_weight_image, angle_height_image], + ] + # centers_lines_points = [(320, 20), (340, 160), (360, 300), (400, 480)] + centers_lines_points = list(zip(lane_centers_in_pixels, self.x_row)) + self.angle = self.heading_car(central_line_points, centers_lines_points) + # print(f"\n\t{angle =}") + # input("press ...") + + ### RESET ################################################ + ## 1. No lateral lines detected + ## 2. REWARD: far from center, vel > target, heading > 30 + ## 3. reach FINISH line + ## 4. Collision + ## + + ## -------- Step() over... + done = False + # print( + # f"\n\t{errors_normalized = }\n\t{self.params['current_speed']=}" + # f"\n\t{self.params['current_steering_angle']=},\n\t{self.params['target_veloc']=}," + # f"\n\t{angle=}" + # ) + + # reward, done, centers_rewards_list = ( + ( + self.center_reward, + self.done_center, + self.centers_rewards_list, + self.velocity_reward, + self.done_velocity, + self.heading_reward, + self.done_heading, + done, + reward, + ) = self.autocarlarewards.rewards_followlane_center_velocity_angle( + self.centers_normal, + self.params["current_speed"], + self.params["target_veloc"], + self.angle, + ) + # print(f"\n\t{reward = }\t{done=}\t{centers_rewards_list=}") + # input(f"\n\tin step() after rewards... waiting") + + ## -------- ... or Finish by... + if len(self.collision_hist) > 0: # crashed you, baby + done = True + reward = -1 + # print(f"crash") + + self.is_finish, self.dist_to_finish = AutoCarlaUtils.finish_fix_number_target( + self.params["location"], + self.finish_alternate_pose, + self.finish_pose_number, + self.max_target_waypoint_distance, + ) + if self.is_finish: + print(f"Finish!!!!") + done = True + + ########################## STATE = DQN INPUTS: (ALL NORMALIZED IN THEIR PARTICULAR RANGES) + + if self.state_space != "image": + self.state = self.DQN_states_simplified_perception_normalized( + self.params["current_speed"], + self.params["target_veloc"], + self.params["current_steering_angle"], + self.angle, + lane_centers_in_pixels, + index_right, + index_left, + mask.shape[1], + ) + else: # IMAGE + mask = self.preprocess_image_lane_detector( + image_rgb_lanedetector_regression, is_image=True + ) + # input(f"{mask.shape=}") + mask = cv2.resize( + mask, (self.new_image_size, self.new_image_size), cv2.INTER_AREA + ) + # input(f"{mask.shape=}") + + self.state = np.array(np.expand_dims(mask, axis=2)) + + self.state = self.concatenate_states(self.state, self.state_space) + + #input(f"end STEP()---> {mask.shape=}") + return self.state, reward, done, {} + + ################################################## + # + # Control + ################################################### + + def control_discrete_actions(self, action): + """ + working with LaneDetector + """ + t = self.new_car.car.get_transform() + v = self.new_car.car.get_velocity() # returns in m/sec + c = ( + self.new_car.car.get_control() + ) # returns values in [-1, 1] range for throttle, steer,...(https://carla.readthedocs.io/en/0.9.13/python_api/#carla.VehicleControl) + w = self.new_car.car.get_angular_velocity() # returns in deg/sec in a 3D vector + a = self.new_car.car.get_acceleration() + + ## Applied throttle, brake and steer + curr_speed = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + curr_steer = math.sqrt(w.x**2 + w.y**2 + w.z**2) + acceleration = math.sqrt(a.x**2 + a.y**2 + a.z**2) + # print(f"in STEP() {throttle = } and {steer = }") + + throttle = self.actions[action][0] + steer = self.actions[action][1] + target_veloc = self.target_veloc + brake = 0 + + # self.params["Steering_angle"] = steering_angle + self.params["location"] = (t.location.x, t.location.y) + self.params["height"] = t.location.z + + self.params["Throttle"] = c.throttle + self.params["Steer"] = c.steer + self.params["Brake"] = c.brake + + self.params["current_speed"] = curr_speed + self.params["current_steering_angle"] = curr_steer + self.params["acceleration"] = acceleration + self.params["target_veloc"] = target_veloc + + # Limiting Velocity up to target_speed = 30 (or any other) + if curr_speed > target_veloc: + throttle = 0.0 + + self.new_car.car.apply_control( + carla.VehicleControl(throttle=throttle, steer=steer, brake=brake) + ) + + ##################################################################################### + # --- methods + ##################################################################################### + + def DQN_states_simplified_perception_normalized( + self, + speed, + target_veloc, + steering, + angle, + lane_centers_in_pixels, + index_right, + index_left, + width, + ): + + v_normal = self.normalizing_DQN_values(speed, target_veloc, 0, is_list=True) + w_normal = self.normalizing_DQN_values(steering, 3, 0, is_list=False) + angle_normal = self.normalizing_DQN_values(angle, 30, 0, is_list=False) + states_normal = self.normalizing_DQN_values( + lane_centers_in_pixels, width, 0, is_list=True + ) + line_borders_normal = self.normalizing_DQN_values( + index_right + index_left, width, 0, is_list=True + ) + + DQN_states_normalized = ( + states_normal + line_borders_normal + v_normal + w_normal + angle_normal + ) + + return DQN_states_normalized + + def normalizing_DQN_values(self, values, max_value, min_value=0, is_list=True): + # values_ = abs(values) + # values_ = values + values = np.array(values).reshape(-1, 1) + min_max_scaler = MinMaxScaler(feature_range=(0, 1)) + min_max_scaler.fit([[min_value], [max_value]]) + data_min_max_scaled = min_max_scaler.transform(values) + values_normal = data_min_max_scaled.flatten().tolist() + values_normal = [ + values_normal[i] if values_normal[i] >= 0 else -1 + for i, _ in enumerate(values_normal) + ] + + return values_normal + + def preprocess_image(self, img): + """ + In only Right Line mode and Sergios segmentation for Town07 + image is trimming from top to middle + """ + ## first, we cut the upper image + height = img.shape[0] + image_middle_line = (height) // 2 + img_sliced = img[image_middle_line:] + ## calculating new image measurements + # height = img_sliced.shape[0] + ## -- convert to GRAY + gray_mask = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2GRAY) + ## -- apply mask to convert in Black and White + theshold = 50 + # _, white_mask = cv2.threshold(gray_mask, 10, 255, cv2.THRESH_BINARY) + _, white_mask = cv2.threshold(gray_mask, theshold, 255, cv2.THRESH_BINARY) + + return white_mask + + def preprocess_image_lane_detector(self, image, is_image=None): + """ + image from lane detector with regression + """ + ## first, we cut the upper image + height = image.shape[0] + image_middle_line = (height) // 2 + img_sliced = image[image_middle_line:] + + if not is_image: + n = 3 + iter = 2 + else: + n = 5 + iter = 4 + + # kernel = np.ones((3, 3), np.uint8) + # img_erosion = cv2.erode(img_sliced, kernel, iterations=2) + kernel = np.ones((n, n), np.uint8) + img_erosion = cv2.erode(img_sliced, kernel, iterations=iter) + + # Convertir la imagen de BGR a RGB + image_rgb = cv2.cvtColor(img_erosion, cv2.COLOR_BGR2RGB) + + # Definir un rango de color en el formato HSV + # lower_blue = np.array([100, 50, 50]) + # upper_blue = np.array([130, 255, 255]) + # lower_red = np.array([0, 50, 50]) + lower_red = np.array([0, 200, 140]) + upper_red = np.array([0, 255, 255]) + # Definir un rango adicional para tonos cercanos al rojo + # lower_red_additional = np.array([170, 50, 50]) + # upper_red_additional = np.array([180, 255, 255]) + + # Convertir la imagen RGB a HSV + image_hsv = cv2.cvtColor(image_rgb, cv2.COLOR_RGB2HSV) + + # Crear una máscara + mask = cv2.inRange(image_hsv, lower_red, upper_red) + + # Crear una máscara para los píxeles rojos y cercanos al rojo + # mask_red = cv2.inRange(image_hsv, lower_red, upper_red) + # mask_red_additional = cv2.inRange( + # image_hsv, lower_red_additional, upper_red_additional + # ) + + # Unir ambas máscaras + # mask = cv2.bitwise_or(mask_red, mask_red_additional) + + # Convertir la máscara a una máscara de tres canales + # mask_rgb = cv2.cvtColor(mask, cv2.COLOR_GRAY2RGB) + + # Copiar la imagen original + # imagen_rgb = np.copy(imagen_rgb) + + # Reemplazar los píxeles azules con blanco en la imagen resultante + # image_rgb[mask == 255] = [255, 255, 255] + # imagen_resultado[mask_rgb == 255] = [255, 255, 255] + + # Mostrar la imagen original y la imagen resultante + # image_rgb = cv2.cvtColor(image_rgb, cv2.COLOR_RGB2BGR) + # image_bgr = cv2.cvtColor(image_rgb, cv2.COLOR_RGB2BGR) + + # cv2.imshow("RGB", image_rgb) + # cv2.imshow("BGR", image_bgr) + + return mask + + def __preprocess_image_lane_detector(self, image): + """ + image from lane detector with regression + """ + ## first, we cut the upper image + height = image.shape[0] + image_middle_line = (height) // 2 + img_sliced = image[image_middle_line:] + + kernel = np.ones((5, 5), np.uint8) + img_erosion = cv2.erode(img_sliced, kernel, iterations=3) + hsv = cv2.cvtColor(img_erosion, cv2.COLOR_RGB2HSV) + gray_hsv = cv2.cvtColor(hsv, cv2.COLOR_BGR2GRAY) + _, gray_mask = cv2.threshold(gray_hsv, 100, 255, cv2.THRESH_BINARY) + + return gray_mask + + def calculate_lane_centers_with_lane_detector(self, gray_mask): + """ + using Lane Detector model for calculating the center + """ + lines = [gray_mask[self.x_row[i], :] for i, _ in enumerate(self.x_row)] + + # lines_conoffset = [lines[i][offset:] for i,_ in enumerate(lines)] + # print(f"{lines=}") + # print(f"{lines_conoffset=}") + index_left = [np.argmax(lines[x]) for x, _ in enumerate(lines)] + # print(f"{index_left = }") + + # substituting 0's by -1 + index_left = [ + index_left[x] if index_left[x] != 0 else 0 for x, _ in enumerate(index_left) + ] + + # calculamos la linea derecha desde la columna 640, desde la derecha + lines_inversed = [list(reversed(lines[x])) for x, _ in enumerate(lines)] + # print(f"{lines_inversed = }") + inv_index_right = [ + np.argmax(lines_inversed[x]) for x, _ in enumerate(lines_inversed) + ] + # print(f"{inv_index_right = }") + index_right = [ + gray_mask.shape[1] - inv_index_right[x] if inv_index_right[x] != 0 else 0 + for x, _ in enumerate(inv_index_right) + ] + # print(f"{index_right = }") + + centers = [ + ((right - left) // 2) + left if right != 0 and left != 0 else 0 + for right, left in zip(index_right, index_left) + ] + dist_to_center = [ + abs(centers[i] - gray_mask.shape[1] // 2) if centers[i] != 0 else 0 + for i, _ in enumerate(centers) + ] + dist_to_center_normalized = [ + ( + float( + ((gray_mask.shape[1] // 2) - centers[i]) / (gray_mask.shape[1] // 2) + ) + if dist_to_center[i] != 0 + else 0 + ) + for i, _ in enumerate(dist_to_center) + ] + return ( + centers, + dist_to_center, + dist_to_center_normalized, + index_right, + index_left, + ) + + def calculate_states( + self, width, lane_centers_in_pixels, num_regions, size_lateral_states + ): + # size_lateral_states = 140 + size_center_states = width - (size_lateral_states * 2) + pixel_center_states = int(size_center_states / (num_regions - 2)) + + states = [ + ( + int(((value - size_lateral_states) / pixel_center_states) + 1) + if (width - size_lateral_states) > value > size_lateral_states + else ( + num_regions - 1 + if value >= (width - size_lateral_states) + else num_regions + ) + ) + for _, value in enumerate(lane_centers_in_pixels) + ] + + ## negatives states due to 0's convert in 0 + states = [states[i] if states[i] > 0 else 0 for i, _ in enumerate(states)] + + # drawing lines and numbers states in image + drawing_lines_states = [ + size_lateral_states + (i * pixel_center_states) + for i in range(1, num_regions - 1) + ] + # self.drawing_lines_states.append(size_lateral_states) + drawing_lines_states.insert(0, size_lateral_states) + + drawing_numbers_states = [ + i if i > 0 else num_regions for i in range(0, num_regions) + ] + + return states, drawing_lines_states, drawing_numbers_states + + def heading_car(self, central_line_points, centers_lines_points): + """ + in general form + central_line_points = [(image.shape[1] //2, 0), [image.shape[1] // 2, image.shape[0]]] + + central_line_points = [(320, 0), [320, 480]] for an image of 480 x 640 dimensions + + + centers_lines_points is variable in size, allows different dimensions + """ + # slope + centers_slope = [ + np.arctan2(y2 - y1, x2 - x1) + for (x1, y1), (x2, y2) in zip( + centers_lines_points[:-1], centers_lines_points[1:] + ) + ] + + # mean angle between 2 lines, central and centers + # angulo_entre_lineas = np.abs(np.degrees(np.mean(pendientes_linea2) - np.arctan2(y2_linea1 - y1_linea1, x2_linea1 - x1_linea1))) + angle = np.abs( + np.degrees( + np.mean(centers_slope) + - np.arctan2( + central_line_points[1][1] - central_line_points[0][1], + central_line_points[1][0] - central_line_points[0][0], + ) + ) + ) + + return angle + + ################## + # + # ONLY RIGHT LANE + #################################################################################### + + def step_only_right_line(self, action): + """ + state: sp + actions: discretes + Only Right Line + """ + + self.control_discrete_actions(action) + + ########### --- calculating STATES + mask = self.preprocess_image(self.sensor_camera_red_mask.front_red_mask_camera) + AutoCarlaUtils.show_image( + "mask", + mask, + 300, + 10, + ) + + ########### --- Calculating center ONLY with right line + ( + errors, + dist_to_center, + dist_to_center_normalized, + index_right, + ground_truth_normal_values, + ) = self.calculate_lane_centers_with_only_right_line(mask) + + print( + f"\n\t{errors = }\n\t{dist_to_center =}\n{dist_to_center_normalized =}\n\t{index_right = }\n\t{ground_truth_normal_values =}" + ) + + ## STATES + ( + self.states, + drawing_lines_states, + drawing_numbers_states, + ) = self.calculate_states( + mask.shape[1], + dist_to_center, + self.num_regions, + size_lateral_states=140, + ) + print( + f"\n\t{self.states = }\n\t{drawing_lines_states =}\n\t{drawing_numbers_states =}" + ) + + index_left = [0 for i, _ in enumerate(index_right)] # only for drawing + AutoCarlaUtils.show_image_lines_centers_borders( + "Front Camera", + self.sensor_camera_rgb.front_rgb_camera[ + (self.sensor_camera_rgb.front_rgb_camera.shape[0] // 2) : + ], + self.x_row, + 800, + 10, + index_right, + index_left, + dist_to_center, + drawing_lines_states, + drawing_numbers_states, + ) + + ## -------- Ending Step()... + ################################### + # + # REWARDS + ################################### + + done = False + reward, done = self.autocarlarewards.rewards_sigmoid_only_right_line( + self.dist_normalized, ground_truth_normal_values, self.params + ) + + return self.states, reward, done, {} + + def calculate_right_line(self, mask, x_row): + """ + All below code has been merged into calculate_lane_centers_with_only_right_line() method + calculates distance from center to right line + This distance will be using as a error from center lane + """ + ## get total lines in every line point + lines = [mask[x_row[i], :] for i, _ in enumerate(x_row)] + + ### ----------------- from right to left + lines_inversed = [list(reversed(lines[x])) for x, _ in enumerate(lines)] + # print(f"{lines_inversed = }") + inv_index_right = [ + np.argmax(lines_inversed[x]) for x, _ in enumerate(lines_inversed) + ] + + index_right = [ + mask.shape[1] - inv_index_right[x] if inv_index_right[x] != 0 else 0 + for x, _ in enumerate(inv_index_right) + ] + + return index_right + + def calculate_lane_centers_with_only_right_line(self, mask): + ## get total lines in every line point + lines = [mask[self.x_row[i], :] for i, _ in enumerate(self.x_row)] + + ### ----------------- from right to left + lines_inversed = [list(reversed(lines[x])) for x, _ in enumerate(lines)] + # print(f"{lines_inversed = }") + inv_index_right = [ + np.argmax(lines_inversed[x]) for x, _ in enumerate(lines_inversed) + ] + + index_right = [ + mask.shape[1] - inv_index_right[x] if inv_index_right[x] != 0 else 0 + for x, _ in enumerate(inv_index_right) + ] + + image_center = mask.shape[1] // 2 + + # TODO: OJO HAY Q DISTINGUIR + Y - DEL CENTRO + dist_to_center = [ + image_center - index_right[i] for i, _ in enumerate(index_right) + ] + + # TODO: VERIFICAR ESTE VALOR + dist_to_center_normalized = [ + float((image_center - index_right[i]) / image_center) + for i, _ in enumerate(index_right) + ] + + ground_truth_normal_values = [ + correct_normalized_distance[value] for i, value in enumerate(self.x_row) + ] + + errors = [ + dist_to_center_normalized[index] - ground_truth_normal_values[index] + for index, _ in enumerate(dist_to_center_normalized) + ] + + return ( + errors, + dist_to_center, + dist_to_center_normalized, + index_right, + ground_truth_normal_values, + ) diff --git a/rl_studio/agents/auto_carla/inference_followlane_qlearn_carla.py b/rl_studio/agents/auto_carla/inference_followlane_qlearn_carla.py new file mode 100755 index 000000000..33f422a77 --- /dev/null +++ b/rl_studio/agents/auto_carla/inference_followlane_qlearn_carla.py @@ -0,0 +1,515 @@ +from datetime import datetime, timedelta +import glob +import os +import time + +import carla +import cv2 +import gymnasium as gym +import numpy as np +import pygame +from reloading import reloading +from tqdm import tqdm + +from rl_studio.agents.f1.loaders import ( + LoadAlgorithmParams, + LoadEnvParams, + LoadEnvVariablesQlearnCarla, + LoadGlobalParams, +) +from rl_studio.agents.utils import ( + render_params, + render_params_left_bottom, + save_dataframe_episodes, + save_carla_dataframe_episodes, + save_carla_latency, + save_batch, + save_best_episode, + LoggingHandler, + print_messages, +) +from rl_studio.algorithms.qlearn import QLearnCarla, QLearn +from rl_studio.envs.gazebo.gazebo_envs import * +from rl_studio.envs.carla.utils.bounding_boxes import BasicSynchronousClient +from rl_studio.envs.carla.utils.logger import logger +from rl_studio.envs.carla.utils.manual_control import HUD, World +from rl_studio.envs.carla.utils.visualize_multiple_sensors import ( + DisplayManager, + SensorManager, +) +from rl_studio.envs.carla.utils.synchronous_mode import ( + CarlaSyncMode, + draw_image, + get_font, + should_quit, +) +from rl_studio.envs.carla.carla_env import CarlaEnv + +try: + sys.path.append( + glob.glob( + "../carla/dist/carla-*%d.%d-%s.egg" + % ( + sys.version_info.major, + sys.version_info.minor, + "win-amd64" if os.name == "nt" else "linux-x86_64", + ) + )[0] + ) +except IndexError: + pass + + +class InferencerFollowLaneQlearnAutoCarla: + """ + Mode: training + Task: Follow Lane + Algorithm: QlearnCarla + Agent: Auto + Simulator: Carla + Weather: Static + Traffic: No + + The most simple environment + """ + + def __init__(self, config): + self.algoritmhs_params = LoadAlgorithmParams(config) + self.env_params = LoadEnvParams(config) + self.environment = LoadEnvVariablesQlearnCarla(config) + self.global_params = LoadGlobalParams(config) + + os.makedirs(f"{self.global_params.models_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.logs_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.metrics_data_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.metrics_graphics_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.recorders_carla_dir}", exist_ok=True) + + self.log_file = f"{self.global_params.logs_dir}/{time.strftime('%Y%m%d-%H%M%S')}_{self.global_params.mode}_{self.global_params.task}_{self.global_params.algorithm}_{self.global_params.agent}_{self.global_params.framework}.log" + + # print(f"\nin TrainerFollowLaneQlearnAutoCarla {config=}") + # print(f"\nin TrainerFollowLaneQlearnAutoCarla {self.environment=}\n") + # print( + # f"\nin TrainerFollowLaneQlearnAutoCarla {self.environment.environment=}\n" + # ) + # lanzamos Carla server + CarlaEnv.__init__(self) + + def main(self): + """ + Qlearn dictionnary + """ + log = LoggingHandler(self.log_file) + env = gym.make(self.env_params.env_name, **self.environment.environment) + + start_time = datetime.now() + best_epoch = 1 + current_max_reward = 0 + best_step = 0 + best_epoch_training_time = 0 + epsilon = self.environment.environment["epsilon"] + epsilon_decay = epsilon / (self.env_params.total_episodes) + # epsilon = epsilon / 2 + # ------------------------------- + # log.logger.info( + # f"\nactions_len = {len(self.global_params.actions_set)}\n" + # f"actions_range = {range(len(self.global_params.actions_set))}\n" + # f"actions = {self.global_params.actions_set}\n" + # f"epsilon = {epsilon}\n" + # f"epsilon_decay = {epsilon_decay}\n" + # f"alpha = {self.environment.environment['alpha']}\n" + # f"gamma = {self.environment.environment['gamma']}\n" + # ) + print_messages( + "main()", + actions_len=len(self.global_params.actions_set), + actions_range=range(len(self.global_params.actions_set)), + actions=self.global_params.actions_set, + epsilon=epsilon, + epsilon_decay=epsilon_decay, + alpha=self.environment.environment["alpha"], + gamma=self.environment.environment["gamma"], + ) + ## --- init Qlearn + qlearn = QLearn( + actions=range(len(self.global_params.actions_set)), + epsilon=self.environment.environment["epsilon"], + alpha=self.environment.environment["alpha"], + gamma=self.environment.environment["gamma"], + ) + ## load q model + qlearn.load_pickle_model( + f"{self.global_params.models_dir}/{self.environment.environment['inference_qlearn_model_name']}" + ) + print(f"{qlearn.q = }") + # print(f"{type(qlearn.q) = }") + # print(f"{qlearn.q[0] = }") + + # log.logger.info( + # f"\nqlearn.q_table = {qlearn.q_table}", + # ) + # print_messages( + # "", + # qlearn_q_table=qlearn.q_table, + # ) + ## ------------- START TRAINING -------------------- + for episode in tqdm( + range(1, self.env_params.total_episodes + 1), + ascii=True, + unit="episodes", + ): + time.sleep(0.1) + done = False + cumulated_reward = 0 + step = 0 + # start_time_epoch = datetime.now() + start_time_epoch = time.time() + + observation = env.reset() + state = "".join(map(str, observation)) + # print_messages( + # "in episode", + # episode=episode, + # observation=observation, + # state=state, + # type_observation=type(observation), + # type_state=type(state), + # ) + + while not done: + # if self.environment.environment["sync"]: + # env.world.tick() + # else: + # env.world.wait_for_tick() + step += 1 + start_step = time.time() + action = qlearn.inference(state) + # print(f"{action = }") + observation, reward, done, _ = env.step(action) + # time.sleep(4) + # print(f"\n{end_step - start_step = }") + cumulated_reward += reward + next_state = "".join(map(str, observation)) + # qlearn.learn(state, action, reward, next_state) + state = next_state + + end_step = time.time() + self.global_params.time_steps[(episode, step)] = end_step - start_step + + """ + print_messages( + "", + episode=episode, + step=step, + action=action, + state=state, + new_observation=new_observation, + reward=reward, + done=done, + next_state=next_state, + current_max_reward=current_max_reward, + ) + """ + """ + log.logger.info( + f"\n step = {step}\n" + f"action = {action}\n" + f"actions type = {type(action)}\n" + f"state = {state}\n" + # f"observation[0]= {observation[0]}\n" + # f"observation type = {type(observation)}\n" + # f"observation[0] type = {type(observation[0])}\n" + f"new_observation = {new_observation}\n" + f"type new_observation = {type(new_observation)}\n" + f"reward = {reward}\n" + f"done = {done}\n" + f"next_state = {next_state}\n" + f"type new_observation = {type(new_observation)}\n" + f"current_max_reward = {current_max_reward}\n" + ) + """ + + # print_messages( + # "", + # next_state=next_state, + # state=state, + # ) + # env.display_manager.render() + # render params + + render_params_left_bottom( + episode=episode, + step=step, + action=action, + epsilon=epsilon, + reward_in_step=reward, + cumulated_reward=cumulated_reward, + _="------------------------", + current_max_reward=current_max_reward, + best_epoch=best_epoch, + best_step=best_step, + done=done, + time_per_step=end_step - start_step, + FPS=1/(end_step - start_step), + ) + + # best episode and step's stats + if current_max_reward <= cumulated_reward: + current_max_reward = cumulated_reward + best_epoch = episode + best_step = step + # best_epoch_training_time = datetime.now() - start_time_epoch + best_epoch_training_time = time.time() - start_time_epoch + self.global_params.actions_rewards["episode"].append(episode) + self.global_params.actions_rewards["step"].append(step) + self.global_params.actions_rewards["reward"].append(reward) + # Showing stats in screen for monitoring. Showing every 'save_every_step' value + if not step % self.env_params.save_every_step: + log.logger.info( + f"SHOWING BATCH OF STEPS\n" + f"current_max_reward = {current_max_reward}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"current epoch = {episode}\n" + f"current step = {step}\n" + f"best epoch so far = {best_epoch}\n" + f"best step so far = {best_step}\n" + f"best_epoch_training_time = {best_epoch_training_time}\n" + ) + # print_messages( + # "SHOWING BATCH OF STEPS", + # episode=episode, + # step=step, + # cumulated_reward=cumulated_reward, + # epsilon=epsilon, + # best_epoch=best_epoch, + # best_step=best_step, + # current_max_reward=current_max_reward, + # best_epoch_training_time=best_epoch_training_time, + # ) + + # Reach Finish Line!!! + if env.is_finish: + np.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_FINISHLINE_Circuit-{self.environment.environment['town']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}-qtable.npy", + qlearn.q, + ) + qlearn.save_qtable_pickle( + self.environment.environment, + self.global_params.models_dir, + qlearn, + cumulated_reward, + episode, + step, + epsilon, + ) + # qlearn.save_model( + # self.environment.environment, + # self.global_params.models_dir, + # qlearn, + # cumulated_reward, + # episode, + # step, + # epsilon, + # ) + + print_messages( + "FINISH LINE", + episode=episode, + step=step, + cumulated_reward=cumulated_reward, + epsilon=epsilon, + ) + log.logger.info( + f"\nFINISH LINE\n" + f"in episode = {episode}\n" + f"steps = {step}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"epsilon = {epsilon}\n" + ) + + # End epoch + if step > self.env_params.estimated_steps: + done = True + np.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['town']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}-qtable.npy", + qlearn.q, + ) + qlearn.save_qtable_pickle( + self.environment.environment, + self.global_params.models_dir, + qlearn, + cumulated_reward, + episode, + step, + epsilon, + ) + # qlearn.save_model( + # self.environment.environment, + # self.global_params.models_dir, + # qlearn, + # cumulated_reward, + # episode, + # step, + # epsilon, + # ) + + print_messages( + "EPISODE COMPLETED", + episode=episode, + step=step, + cumulated_reward=cumulated_reward, + epsilon=epsilon, + ) + log.logger.info( + f"\nEPISODE COMPLETED\n" + f"in episode = {episode}\n" + f"steps = {step}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"epsilon = {epsilon}\n" + ) + + # check out for Carla Server (end of every step) + ## ----------- checking for Carla Server is working + env.checking_carla_server(self.environment.environment["town"]) + + ## showing q_table every determined steps + if not episode % self.env_params.save_episodes: + print(f"\n\tQ-table {qlearn.q}") + + # Save best lap + if ( + cumulated_reward - self.environment.environment["rewards"]["penal"] + ) >= current_max_reward: + self.global_params.best_current_epoch["best_epoch"].append(best_epoch) + self.global_params.best_current_epoch["highest_reward"].append( + cumulated_reward + ) + self.global_params.best_current_epoch["best_step"].append(best_step) + self.global_params.best_current_epoch[ + "best_epoch_training_time" + ].append(best_epoch_training_time) + self.global_params.best_current_epoch[ + "current_total_training_time" + ].append(datetime.now() - start_time) + save_carla_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.best_current_epoch, + ) + # save_carla_latency(self.environment.environment, self.global_params.metrics_data_dir, self.global_params.time_steps) + + np.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['town']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward - self.environment.environment['rewards']['penal'])}-qtable.npy", + qlearn.q, + ) + qlearn.save_qtable_pickle( + self.environment.environment, + self.global_params.models_dir, + qlearn, + cumulated_reward, + episode, + step, + epsilon, + ) + log.logger.info( + f"\nsaving best lap\n" + f"in episode = {episode}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"current_max_reward = {current_max_reward}\n" + f"steps = {step}\n" + f"epsilon = {epsilon}\n" + ) + # print_messages( + # "saving best lap", + # episode=episode, + # cumulated_reward=cumulated_reward, + # current_max_reward=current_max_reward, + # step=step, + # epsilon=epsilon, + # ) + + # end of training by: + # training time setting: 2 hours, 15 hours... + # num epochs + + if ( + datetime.now() - timedelta(hours=self.global_params.training_time) + > start_time + ) or (episode > self.env_params.total_episodes): + if ( + cumulated_reward - self.environment.environment["rewards"]["penal"] + ) >= current_max_reward: + np.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['town']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}-qtable.npy", + qlearn.q, + ) + qlearn.save_qtable_pickle( + self.environment.environment, + self.global_params.models_dir, + qlearn, + cumulated_reward, + episode, + step, + epsilon, + ) + log.logger.info( + f"\nTraining Time over\n" + f"current_max_reward = {current_max_reward}\n" + f"epoch = {episode}\n" + f"step = {step}\n" + f"epsilon = {epsilon}\n" + ) + # print_messages( + # "Training Time over", + # episode=episode, + # cumulated_reward=cumulated_reward, + # current_max_reward=current_max_reward, + # step=step, + # epsilon=epsilon, + # ) + break + + # save best values every save_episode times + self.global_params.ep_rewards.append(cumulated_reward) + if not episode % self.env_params.save_episodes: + self.global_params.aggr_ep_rewards = save_batch( + episode, + step, + start_time_epoch, + start_time, + self.global_params, + self.env_params, + ) + save_carla_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.aggr_ep_rewards, + ) + log.logger.info( + f"\nsaving BATCH\n" + f"current_max_reward = {current_max_reward}\n" + f"best_epoch = {best_epoch}\n" + f"best_step = {best_step}\n" + f"best_epoch_training_time = {best_epoch_training_time}\n" + ) + # print_messages( + # "saving BATCH", + # best_epoch=best_epoch, + # best_epoch_training_time=best_epoch_training_time, + # current_max_reward=current_max_reward, + # best_step=best_step, + # ) + # updating epsilon for exploration + if epsilon > self.environment.environment["epsilon_min"]: + # self.epsilon *= self.epsilon_discount + epsilon -= epsilon_decay + epsilon = qlearn.updateEpsilon( + max(self.environment.environment["epsilon_min"], epsilon) + ) + + ## ------------ destroy actors + env.destroy_all_actors() + # env.display_manager.destroy() + # ----------- end for + + # env.close() diff --git a/rl_studio/agents/auto_carla/rewards.py b/rl_studio/agents/auto_carla/rewards.py new file mode 100644 index 000000000..f01921393 --- /dev/null +++ b/rl_studio/agents/auto_carla/rewards.py @@ -0,0 +1,168 @@ +import math + +import numpy as np + + +class AutoCarlaRewards: + + def rewards_followlane_center_velocity_angle( + self, centers_norm, velocity, target_vel, angle + ): + """ + rewards = f(center, vel, angle) = center + velocity - heading + + """ + + w_center = 1 + w_velocity = 1 + w_heading = 1 + done_function = False + + center_reward, done_center, centers_rewards_list = self.rewards_center( + centers_norm + ) + velocity_reward, done_velocity = self.rewards_velocity(velocity, target_vel) + heading_reward, done_heading = self.rewards_heading(angle) + + # print(f"\n\tin rewards function()....") + # print(f"\t{center_reward=}, {done_center=}, {centers_rewards_list=}") + # print(f"\t{velocity_reward=}, {done_velocity=}") + # print(f"\t{heading_reward=}, {done_heading=}") + + function_reward = ( + (w_center * center_reward) + + (w_velocity * velocity_reward) + + (w_heading * heading_reward) + ) + if done_center or done_velocity or done_heading: + done_function = True + + return ( + center_reward, + done_center, + centers_rewards_list, + velocity_reward, + done_velocity, + heading_reward, + done_heading, + done_function, + function_reward, + ) + # return function_reward, done_function, centers_rewards_list + + def rewards_center(self, centers_norm): + """ + Sigmoid + rewards in x = [0, 1] and y = [0, 1] + a - (b / (c + d * exp(-i * center_norm))) + """ + + a = 1.078 + b = 0.108 + c = 0.1 + d = 1.3 + i = 8.7 + + rewards = [] + done = False + # for index, _ in enumerate(centers_norm): + # dist = dist_normalized[index] - ground_truth_normalized[index] + # rewards.append(a - (b / (c + d * math.exp(-i * centers_norm[index])))) + rewards = [ + a - (b / (c + d * math.exp(-i * abs(centers_norm[index])))) + for index, _ in enumerate(centers_norm) + ] + + function_reward = sum(rewards) / len(rewards) + # dist_normaliz_mean = sum(dist_normalized) / len(dist_normalized) + + # if function_reward < 0.1: #por dist_normaliz_mean >= 0.16 or dist_normaliz_mean <= -0.5): # distance of -0.8 to right, and 0.6 to left + # if max([abs(value) for value in centers_norm]) > max_dist2center_allowed: #por dist_normaliz_mean >= 0.16 or dist_normaliz_mean <= -0.5): # distance of -0.8 to right, and 0.6 to left + if ( + function_reward < 0.4 + ): # por dist_normaliz_mean >= 0.16 or dist_normaliz_mean <= -0.5): # distance of -0.8 to right, and 0.6 to left + done = True + done = True + # function_reward = 0 + + return function_reward, done, rewards + + def rewards_velocity(self, velocity, target_vel=50): + """ + From vel = [0, target_vel] is a increasing linear function from [0,1] + From vel > target_vel, the function is a sharply decreasing sigmoid + """ + done = False + # x_values = np.linspace(0, max_veloc, 1000) + + if ( + velocity <= target_vel + ): # up to target_vel the function is a simple increasing linear function + reward = (velocity - 0) / (target_vel - 0) + else: # after target_vel ( ie. > 50 the function is decreasing sharply sigmoid ) + # return 1.0 - (x - 50) / (200 - 50) + # return 1 / (1 + np.exp((x - 95) / 14.4)) + reward = 1.0 - (1 / (1 + np.exp(-0.5 * (velocity - target_vel)))) + + # if reward < 0.1: + if not (0 <= velocity <= target_vel + 5): + done = True + + return reward, done + + def rewards_heading(self, angle, max_angle=30): + """ + rewards in x = [0, 1] and y = [0, 1] + It is a sigmoid function + """ + + a = 1 + b = 1 + c = 1 + d = -12.4 + i = 0.4 + + # rewards = [] + done = False + angle_normal = angle / max_angle + + reward = a - (b / (c + math.exp(d * (abs(angle_normal) - i)))) + + if abs(angle_normal) >= 0.9: + done = True + + return reward, done + + ###################################### + + def rewards_followlane_two_lines(self, errors_normalized, center_image, params): + """ + Lines right and left, such as LaneDetector or Sergios segmentation + N points of perception + + rewards in x = [0, 1] and y = [0, 10] + 10.099 - (1.1 /(0.1089 + 10e(-12 * dist))) + """ + + a = 10.099 + b = 1.1 + c = 0.1089 + d = 10 + e = 12 + + rewards = [] + done = False + for index, _ in enumerate(errors_normalized): + dist = errors_normalized[index] - center_image + rewards.append(a - (b / (c + d * math.exp(-e * abs(dist))))) + + function_reward = sum(rewards) / len(rewards) + + if function_reward < 0.5: + done = True + + # TODO: remove next comments to add new variables in reward function + # function_reward += params["velocity"] * 0.5 + # function_reward -= params["steering_angle"] * 1.02 + + return function_reward, done diff --git a/rl_studio/agents/auto_carla/settings.py b/rl_studio/agents/auto_carla/settings.py new file mode 100644 index 000000000..28e4b95d9 --- /dev/null +++ b/rl_studio/agents/auto_carla/settings.py @@ -0,0 +1,77 @@ +from pydantic import BaseModel + +from rl_studio.agents.auto_carla.rewards import AutoCarlaRewards + +# from rl_studio.envs.carla.followlane.rewards import AutoCarlaRewards +# from rl_studio.envs.carla.followlane.simplified_perception import ( +# AutoCarlaSimplifiedPerception, +# ) +from rl_studio.agents.auto_carla.utils import AutoCarlaUtils + + +class FollowLaneCarlaConfig(BaseModel): + def __init__(self, **config): + # self.simplifiedperception = AutoCarlaSimplifiedPerception() + self.autocarlarewards = AutoCarlaRewards() + self.autocarlautils = AutoCarlaUtils() + # self.autocarlalanedetector = LaneDetector() + # self.autocarlainitcarpose = AutoCarlaInitCarPose() + + # self.image = ImageF1() + # self.image = ListenerCamera("/F1ROS/cameraL/image_raw") + self.image_raw_from_topic = None + self.image_camera = None + # self.sensor = config["sensor"] + + # Image + self.image_resizing = config["image_resizing"] / 100 + self.new_image_size = config["new_image_size"] + self.raw_image = config["raw_image"] + self.height = int(config["height_image"] * self.image_resizing) + self.width = int(config["width_image"] * self.image_resizing) + self.center_image = int(config["center_image"] * self.image_resizing) + self.num_regions = config["num_regions"] + self.pixel_region = int(self.center_image / self.num_regions) * 2 + # self.telemetry_mask = config["telemetry_mask"] + self.poi = config["x_row"][0] + self.image_center = None + self.right_lane_center_image = config["center_image"] + ( + config["center_image"] // 2 + ) + self.lower_limit = config["lower_limit"] + + # States + self.state_space = config["states"] + self.states_entry = config["states_entry"] + if self.state_space == "spn": + self.x_row = [i for i in range(1, int(self.height / 2) - 1)] + else: + self.x_row = config["x_row"] + + # Actions + self.actions_space = config["action_space"] + self.actions = config["actions"] + + # Rewards + self.reward_function = config["reward_function"] + self.rewards = config["rewards"] + self.min_reward = config["min_reward"] + + # Pose + self.town = config["town"] + self.random_pose = config["random_pose"] + self.alternate_pose = config["alternate_pose"] + self.init_pose_number = config["init_pose_number"] + self.finish_pose_number = config["finish_pose_number"] + self.start_alternate_pose = config["start_alternate_pose"] + self.finish_alternate_pose = config["finish_alternate_pose"] + + self.waypoints_meters = config["waypoints_meters"] + self.waypoints_init = config["waypoints_init"] + self.waypoints_target = config["waypoints_target"] + self.waypoints_lane_id = config["waypoints_lane_id"] + self.waypoints_road_id = config["waypoints_road_id"] + self.max_target_waypoint_distance = config["max_target_waypoint_distance"] + + # self.actor_list = [] + # self.collision_hist = [] diff --git a/rl_studio/agents/auto_carla/train_followlane_carla_sb3.py b/rl_studio/agents/auto_carla/train_followlane_carla_sb3.py new file mode 100755 index 000000000..0b4a6468c --- /dev/null +++ b/rl_studio/agents/auto_carla/train_followlane_carla_sb3.py @@ -0,0 +1,1126 @@ +from collections import Counter, OrderedDict +from datetime import datetime, timedelta +import glob +import math +from statistics import median +import os +import random +import time +import weakref + +import carla +import cv2 +import gymnasium as gym +from gymnasium import spaces +import numpy as np +import pygame +from reloading import reloading +import tensorflow as tf +from tqdm import tqdm +from stable_baselines3.common.env_checker import check_env +import sys + +from rl_studio.agents.f1.loaders import ( + LoadAlgorithmParams, + LoadEnvParams, + LoadEnvVariablesDDPGCarla, + LoadEnvVariablesSB3Carla, + LoadGlobalParams, +) +from rl_studio.agents.utils import ( + render_params, + render_params_left_bottom, + save_dataframe_episodes, + save_carla_dataframe_episodes, + save_batch, + save_best_episode, + LoggingHandler, + print_messages, +) +from rl_studio.agents.utilities.plot_stats import MetricsPlot, StatsDataFrame +from rl_studio.algorithms.ddpg import ( + ModifiedTensorBoard, + OUActionNoise, + Buffer, + DDPGAgent, +) +from rl_studio.algorithms.dqn_keras import ( + ModifiedTensorBoard, + DQN, +) +from rl_studio.algorithms.utils import ( + save_actorcritic_model, +) + +# from rl_studio.envs.gazebo.gazebo_envs import * +from rl_studio.envs.carla.carla_env import CarlaEnv +from rl_studio.envs.carla.followlane.utils import AutoCarlaUtils +from rl_studio.envs.carla.followlane.settings import FollowLaneCarlaConfig +from rl_studio.envs.carla.utils.bounding_boxes import BasicSynchronousClient +from rl_studio.envs.carla.utils.logger import logger +from rl_studio.envs.carla.utils.manual_control import HUD, World +from rl_studio.envs.carla.utils.visualize_multiple_sensors import ( + DisplayManager, + SensorManager, +) +from rl_studio.envs.carla.utils.synchronous_mode import ( + CarlaSyncMode, + draw_image, + get_font, + should_quit, +) + + +try: + sys.path.append( + glob.glob( + "../carla/dist/carla-*%d.%d-%s.egg" + % ( + sys.version_info.major, + sys.version_info.minor, + "win-amd64" if os.name == "nt" else "linux-x86_64", + ) + )[0] + ) +except IndexError: + pass + + +# Sharing GPU +gpus = tf.config.experimental.list_physical_devices("GPU") +for gpu in gpus: + tf.config.experimental.set_memory_growth(gpu, True) + + +correct_normalized_distance = { + 20: -0.07, + 30: -0.1, + 40: -0.13, + 50: -0.17, + 60: -0.2, + 70: -0.23, + 80: -0.26, + 90: -0.3, + 100: -0.33, + 110: -0.36, + 120: -0.4, + 130: -0.42, + 140: -0.46, + 150: -0.49, + 160: -0.52, + 170: -0.56, + 180: -0.59, + 190: -0.62, + 200: -0.65, + 210: -0.69, + 220: -0.72, +} +correct_pixel_distance = { + 20: 343, + 30: 353, + 40: 363, + 50: 374, + 60: 384, + 70: 394, + 80: 404, + 90: 415, + 100: 425, + 110: 436, + 120: 446, + 130: 456, + 140: 467, + 150: 477, + 160: 488, + 170: 498, + 180: 508, + 190: 518, + 200: 528, + 210: 540, + 220: 550, +} + + +class CarlaEnv(gym.Env): + def __init__(self, car, sensor_camera_red_mask, config): + super(CarlaEnv, self).__init__() + ## --------------- init env + # FollowLaneEnv.__init__(self, **config) + ## --------------- init class variables + FollowLaneCarlaConfig.__init__(self, **config) + + # print(f"{config =}") + + self.image_raw_from_topic = None + self.image_camera = None + # self.sensor = config["sensor"] + + # Image + self.image_resizing = config["image_resizing"] / 100 + self.new_image_size = config["new_image_size"] + self.raw_image = config["raw_image"] + self.height = int(config["height_image"] * self.image_resizing) + self.width = int(config["width_image"] * self.image_resizing) + self.center_image = int(config["center_image"] * self.image_resizing) + self.num_regions = config["num_regions"] + self.pixel_region = int(self.center_image / self.num_regions) * 2 + # self.telemetry_mask = config["telemetry_mask"] + self.poi = config["x_row"][0] + self.image_center = None + self.right_lane_center_image = config["center_image"] + ( + config["center_image"] // 2 + ) + self.lower_limit = config["lower_limit"] + + # States + self.state_space = config["states"] + self.states_entry = config["states_entry"] + if self.state_space == "spn": + self.x_row = [i for i in range(1, int(self.height / 2) - 1)] + else: + self.x_row = config["x_row"] + + # Actions + self.actions_space = config["action_space"] + self.actions = config["actions"] + + # Rewards + self.reward_function = config["reward_function"] + self.rewards = config["rewards"] + self.min_reward = config["min_reward"] + + # Pose + self.town = config["town"] + self.random_pose = config["random_pose"] + self.alternate_pose = config["alternate_pose"] + self.init_pose_number = config["init_pose_number"] + self.finish_pose_number = config["finish_pose_number"] + self.start_alternate_pose = config["start_alternate_pose"] + self.finish_alternate_pose = config["finish_alternate_pose"] + + # print(f"{self.alternate_pose =}\n") + # print(f"{self.start_alternate_pose =}\n") + + self.waypoints_meters = config["waypoints_meters"] + self.waypoints_init = config["waypoints_init"] + self.waypoints_target = config["waypoints_target"] + self.waypoints_lane_id = config["waypoints_lane_id"] + self.waypoints_road_id = config["waypoints_road_id"] + self.max_target_waypoint_distance = config["max_target_waypoint_distance"] + + ############################################################### + # + # gym/stable-baselines3 interface + ############################################################### + + ######## Actions Gym based + print(f"{self.state_space = } and {self.actions_space =}") + # print(f"{len(self.actions) =}") + # print(f"{type(self.actions) =}") + # print(f"{self.actions =}") + # Discrete Actions + if self.actions_space == "carla_discrete": + self.action_space = spaces.Discrete(len(self.actions)) + else: # Continuous Actions + actions_to_array = np.array( + [list(self.actions["v"]), list(self.actions["w"])] + ) + print(f"{actions_to_array =}") + print(f"{actions_to_array[0] =}") + print(f"{actions_to_array[1] =}") + self.action_space = spaces.Box( + low=actions_to_array[0], + high=actions_to_array[1], + shape=(2,), + dtype=np.float32, + ) + print(f"{self.action_space.low = }") + print(f"{self.action_space.high = }") + print(f"{self.action_space.low[0] = }") + print(f"{self.action_space.high[0] = }") + + print(f"{self.action_space =}") + + ######## observations Gym based + # image + if self.state_space == "image": + self.observation_space = spaces.Box( + low=0, high=255, shape=(self.height, self.width, 3), dtype=np.uint8 + ) + else: # Discrete observations vector = [0.0, 3.6, 8.9] + # TODO: change x_row for other list + self.observation_space = spaces.Discrete(len(self.x_row)) # temporary + + print(f"{self.observation_space = }") + + ######################################################################### + + # self.params = config + # print(f"{self.params =}\n") + + self.car = car + self.sensor_camera_red_mask = sensor_camera_red_mask + self.params = {} + self.is_finish = None + self.dist_to_finish = None + self.collision_hist = [] + + self.right_line_in_pixels = None + self.ground_truth_pixel_values = None + self.dist_normalized = None + self.states = None + self.state_right_lines = None + self.drawing_lines_states = [] + self.drawing_numbers_states = [] + + ##################################################################################### + # + # RESET + # + ##################################################################################### + def reset(self, seed=None, options=None): + """ + state = vector / simplified perception + actions = discrete + """ + + ############################################################################ + ########### --- calculating STATES + while self.sensor_camera_red_mask.front_red_mask_camera is None: + # print( + # f"RESET() ----> {self.sensor_camera_red_mask.front_red_mask_camera = }" + # ) + time.sleep(1) + + # mask = self.preprocess_image(self.front_red_mask_camera) + mask = self.preprocess_image(self.sensor_camera_red_mask.front_red_mask_camera) + self.right_line_in_pixels = self.calculate_right_line(mask, self.x_row) + + pixels_in_state = mask.shape[1] / self.num_regions + self.states = [ + int(value / pixels_in_state) + for _, value in enumerate(self.right_line_in_pixels) + ] + # states = [5, 5, 5, 5] + states_size = len(self.states) + + # print(f"{states =} and {states_size =}") + return self.states, states_size + + ##################################################################################### + # + # STEP + # + ##################################################################################### + def step(self, action): + """ + state: sp + actions: continuous + only right line + """ + + self.control_discrete_actions_only_right(action) + + ########### --- calculating STATES + mask = self.preprocess_image(self.sensor_camera_red_mask.front_red_mask_camera) + + ########### --- Calculating center ONLY with right line + self.right_line_in_pixels = self.calculate_right_line(mask, self.x_row) + + image_center = mask.shape[1] // 2 + # dist = [ + # image_center - right_line_in_pixels[i] + # for i, _ in enumerate(right_line_in_pixels) + # ] + self.dist_normalized = [ + float((image_center - self.right_line_in_pixels[i]) / image_center) + for i, _ in enumerate(self.right_line_in_pixels) + ] + + ## STATES + + # pixels_in_state = mask.shape[1] // self.num_regions + # self.state_right_lines = [ + # i for i in range(1, mask.shape[1]) if i % pixels_in_state == 0 + # ] + # self.states = [ + # int(value / pixels_in_state) + # for _, value in enumerate(self.right_line_in_pixels) + # ] + + # non regular states: 1 to n-2 in center. n-1 right, n left + size_lateral_states = 140 + size_center_states = mask.shape[1] - (size_lateral_states * 2) + pixel_center_states = int(size_center_states / (self.num_regions - 2)) + + self.states = [ + int(((value - size_lateral_states) / pixel_center_states) + 1) + if (mask.shape[1] - size_lateral_states) > value > size_lateral_states + else self.num_regions - 1 + if value >= (mask.shape[1] - size_lateral_states) + else self.num_regions + for _, value in enumerate(self.right_line_in_pixels) + ] + + # drawing lines and numbers states in image + self.drawing_lines_states = [ + size_lateral_states + (i * pixel_center_states) + for i in range(1, self.num_regions - 1) + ] + # self.drawing_lines_states.append(size_lateral_states) + self.drawing_lines_states.insert(0, size_lateral_states) + + self.drawing_numbers_states = [ + i if i > 0 else self.num_regions for i in range(0, self.num_regions) + ] + # print(f"\n{self.drawing_lines_states}") + # print(f"\n{self.drawing_numbers_states}") + + ## -------- Ending Step()... + done = False + ground_truth_normal_values = [ + correct_normalized_distance[value] for i, value in enumerate(self.x_row) + ] + # reward, done = self.autocarlarewards.rewards_right_line( + # dist_normalized, ground_truth_normal_values, self.params + # ) + reward, done = self.autocarlarewards.rewards_sigmoid_only_right_line( + self.dist_normalized, ground_truth_normal_values + ) + + ## -------- ... or Finish by... + if len(self.collision_hist) > 0: # crashed you, baby + done = True + # reward = -100 + print(f"crash") + + self.is_finish, self.dist_to_finish = AutoCarlaUtils.finish_fix_number_target( + self.params["location"], + self.finish_alternate_pose, + self.finish_pose_number, + self.max_target_waypoint_distance, + ) + if self.is_finish: + print(f"Finish!!!!") + done = True + + self.ground_truth_pixel_values = [ + correct_pixel_distance[value] for i, value in enumerate(self.x_row) + ] + + return self.states, reward, done, {} + + ################################################## + # + # Control + ################################################### + + def control_discrete_actions_only_right(self, action): + t = self.car.car.get_transform() + v = self.car.car.get_velocity() + c = self.car.car.get_control() + w = self.car.car.get_angular_velocity() + a = self.car.car.get_acceleration() + self.params["speed"] = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + self.params["steering_angle"] = w + # self.params["Steering_angle"] = steering_angle + self.params["Steer"] = c.steer + self.params["location"] = (t.location.x, t.location.y) + self.params["Throttle"] = c.throttle + self.params["Brake"] = c.brake + self.params["height"] = t.location.z + self.params["Acceleration"] = math.sqrt(a.x**2 + a.y**2 + a.z**2) + + ## Applied throttle, brake and steer + curr_speed = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + target_speed = 30 + # throttle_low_limit = 0.0 #0.1 for low speeds + brake = 0 + + throttle = self.actions[action][0] + steer = self.actions[action][1] + # print(f"in STEP() {throttle = } and {steer = }") + + if curr_speed > target_speed: + throttle = 0.45 + + self.car.car.apply_control( + carla.VehicleControl(throttle=throttle, steer=steer, brake=brake) + ) + + ##################################################################################### + # --- methods + ##################################################################################### + + def preprocess_image(self, img): + """ + image is trimming from top to middle + """ + ## first, we cut the upper image + height = img.shape[0] + image_middle_line = (height) // 2 + img_sliced = img[image_middle_line:] + ## calculating new image measurements + # height = img_sliced.shape[0] + ## -- convert to GRAY + gray_mask = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2GRAY) + ## -- apply mask to convert in Black and White + theshold = 50 + # _, white_mask = cv2.threshold(gray_mask, 10, 255, cv2.THRESH_BINARY) + _, white_mask = cv2.threshold(gray_mask, theshold, 255, cv2.THRESH_BINARY) + + return white_mask + + def calculate_right_line(self, mask, x_row): + """ + calculates distance from center to right line + This distance will be using as a error from center lane + """ + ## get total lines in every line point + lines = [mask[x_row[i], :] for i, _ in enumerate(x_row)] + + ### ----------------- from right to left + lines_inversed = [list(reversed(lines[x])) for x, _ in enumerate(lines)] + # print(f"{lines_inversed = }") + inv_index_right = [ + np.argmax(lines_inversed[x]) for x, _ in enumerate(lines_inversed) + ] + + index_right = [ + mask.shape[1] - inv_index_right[x] if inv_index_right[x] != 0 else 0 + for x, _ in enumerate(inv_index_right) + ] + + return index_right + + +# ============================================================================== +# -- class RGB CameraManager ------------------------------------------------------------- +# ============================================================================== +class CameraRGBSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.front_rgb_camera = None + + self.world = self._parent.get_world() + bp = self.world.get_blueprint_library().find("sensor.camera.rgb") + bp.set_attribute("image_size_x", f"640") + bp.set_attribute("image_size_y", f"480") + bp.set_attribute("fov", f"110") + + self.sensor = self.world.spawn_actor( + bp, + carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)), + attach_to=self._parent, + ) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda image: CameraRGBSensor._rgb_image(weak_self, image)) + + @staticmethod + def _rgb_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + image = np.array(image.raw_data) + image = image.reshape((480, 640, 4)) + # image = image.reshape((512, 1024, 4)) + image = image[:, :, :3] + # self._data_dict["image"] = image3 + self.front_rgb_camera = image + + +# ============================================================================== +# -- Red Mask CameraManager ------------------------------------------------------------- +# ============================================================================== +class CameraRedMaskSemanticSensor(object): + def __init__(self, parent_actor): + self.sensor = None + self._parent = parent_actor + self.front_red_mask_camera = None + + self.world = self._parent.get_world() + bp = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + bp.set_attribute("image_size_x", f"640") + bp.set_attribute("image_size_y", f"480") + bp.set_attribute("fov", f"110") + + self.sensor = self.world.spawn_actor( + bp, + carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)), + attach_to=self._parent, + ) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda image: CameraRedMaskSemanticSensor._red_mask_semantic_image_callback( + weak_self, image + ) + ) + + @staticmethod + def _red_mask_semantic_image_callback(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + hsv_nemo = cv2.cvtColor(array, cv2.COLOR_RGB2HSV) + + if ( + self.world.get_map().name == "Carla/Maps/Town07" + or self.world.get_map().name == "Carla/Maps/Town04" + or self.world.get_map().name == "Carla/Maps/Town07_Opt" + or self.world.get_map().name == "Carla/Maps/Town04_Opt" + ): + light_sidewalk = (42, 200, 233) + dark_sidewalk = (44, 202, 235) + else: + light_sidewalk = (151, 217, 243) + dark_sidewalk = (153, 219, 245) + + light_pavement = (149, 127, 127) + dark_pavement = (151, 129, 129) + + mask_sidewalk = cv2.inRange(hsv_nemo, light_sidewalk, dark_sidewalk) + # result_sidewalk = cv2.bitwise_and(array, array, mask=mask_sidewalk) + + mask_pavement = cv2.inRange(hsv_nemo, light_pavement, dark_pavement) + # result_pavement = cv2.bitwise_and(array, array, mask=mask_pavement) + + # Adjust according to your adjacency requirement. + kernel = np.ones((3, 3), dtype=np.uint8) + + # Dilating masks to expand boundary. + color1_mask = cv2.dilate(mask_sidewalk, kernel, iterations=1) + color2_mask = cv2.dilate(mask_pavement, kernel, iterations=1) + + # Required points now will have both color's mask val as 255. + common = cv2.bitwise_and(color1_mask, color2_mask) + SOME_THRESHOLD = 0 + + # Common is binary np.uint8 image, min = 0, max = 255. + # SOME_THRESHOLD can be anything within the above range. (not needed though) + # Extract/Use it in whatever way you want it. + intersection_points = np.where(common > SOME_THRESHOLD) + + # Say you want these points in a list form, then you can do this. + pts_list = [[r, c] for r, c in zip(*intersection_points)] + # print(pts_list) + + # for x, y in pts_list: + # image_2[x][y] = (255, 0, 0) + + # red_line_mask = np.zeros((400, 500, 3), dtype=np.uint8) + red_line_mask = np.zeros((image.height, image.width, 3), dtype=np.uint8) + + for x, y in pts_list: + red_line_mask[x][y] = (255, 0, 0) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + red_line_mask = cv2.cvtColor(red_line_mask, cv2.COLOR_BGR2RGB) + self.front_red_mask_camera = red_line_mask + + # AutoCarlaUtils.show_image( + # "states", + # self.front_red_mask_camera, + # 600, + # 400, + # ) + # if self.front_red_mask_camera is not None: + # time.sleep(0.01) + # print(f"self.front_red_mask_camera leyendo") + # print(f"in _red_mask_semantic_image_callback() {time.time()}") + + +# ============================================================================== +# -- New Car ------------------------------------------------------------- +# ============================================================================== +class NewCar(object): + def __init__(self, parent_actor, init_positions, init=None): + self.car = None + self._parent = parent_actor + self.world = self._parent + vehicle = self.world.get_blueprint_library().filter("vehicle.*")[0] + + if init is None: + pose_init = np.random.randint(0, high=len(init_positions)) + else: + pose_init = init + + location = carla.Transform( + carla.Location( + x=init_positions[pose_init][0], + y=init_positions[pose_init][1], + z=init_positions[pose_init][2], + ), + carla.Rotation( + pitch=init_positions[pose_init][3], + yaw=init_positions[pose_init][4], + roll=init_positions[pose_init][5], + ), + ) + + self.car = self.world.spawn_actor(vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(vehicle, location) + + +############################################################################################# +# +# Trainer +############################################################################################# + + +class TrainerFollowLaneAutoCarlaSB3: + """ + Mode: training + Task: Follow Lane + Algorithm: any of Stable-Baselines3 + Agent: Auto + Simulator: Carla + Framework: Stable-Baselines3 + Weather: Static + Traffic: No + + The most simplest environment + """ + + def __init__(self, config): + # print(f"{config =}\n") + + self.algoritmhs_params = LoadAlgorithmParams(config) + self.env_params = LoadEnvParams(config) + self.environment = LoadEnvVariablesSB3Carla(config) + self.global_params = LoadGlobalParams(config) + + os.makedirs(f"{self.global_params.models_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.logs_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.metrics_data_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.metrics_graphics_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.recorders_carla_dir}", exist_ok=True) + + self.log_file = f"{self.global_params.logs_dir}/{time.strftime('%Y%m%d-%H%M%S')}_{self.global_params.mode}_{self.global_params.task}_{self.global_params.algorithm}_{self.global_params.agent}_{self.global_params.framework}.log" + + # CarlaEnv.__init__(self) + + self.world = None + self.client = None + self.front_rgb_camera = None + self.front_red_mask_camera = None + self.actor_list = [] + + ######################################################################### + # Main + ######################################################################### + + def main(self): + """ """ + + ######################################################################### + # Vars + ######################################################################### + + log = LoggingHandler(self.log_file) + random.seed(1) + np.random.seed(1) + tf.compat.v1.random.set_random_seed(1) + + start_time = datetime.now() + best_epoch = 1 + current_max_reward = 0 + best_step = 0 + best_epoch_training_time = 0 + epsilon = self.algoritmhs_params.epsilon + epsilon_discount = self.algoritmhs_params.epsilon_discount + epsilon_min = self.algoritmhs_params.epsilon_min + epsilon_decay = epsilon / (self.env_params.total_episodes // 2) + + dqn_agent = DQN( + self.environment.environment, + self.algoritmhs_params, + len(self.global_params.actions_set), + len(self.environment.environment["x_row"]), + self.global_params.models_dir, + self.global_params, + ) + # Init TensorBoard + tensorboard = ModifiedTensorBoard( + log_dir=f"{self.global_params.logs_tensorboard_dir}/{self.algoritmhs_params.model_name}-{time.strftime('%Y%m%d-%H%M%S')}" + ) + + try: + ######################################################################### + # Vars + ######################################################################### + # print(f"\n al inicio de Main() {self.environment.environment =}\n") + self.client = carla.Client( + self.environment.environment["carla_server"], + self.environment.environment["carla_client"], + ) + self.client.set_timeout(3.0) + print(f"\n maps in carla 0.9.13: {self.client.get_available_maps()}\n") + + self.world = self.client.load_world(self.environment.environment["town"]) + + # Sync mode + settings = self.world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.1 # read: https://carla.readthedocs.io/en/0.9.13/adv_synchrony_timestep/# Phisics substepping + self.world.apply_settings(settings) + + # Set up the traffic manager + traffic_manager = self.client.get_trafficmanager(8000) + traffic_manager.set_synchronous_mode(True) + traffic_manager.set_random_device_seed(0) # define TM seed for determinism + + self.client.reload_world(False) + + # print(f"{settings.synchronous_mode =}") + # self.world.tick() + # print(f"{self.world.tick()=}") + + # Town07 take layers off + if self.environment.environment["town"] == "Town07_Opt": + self.world.unload_map_layer(carla.MapLayer.Buildings) + self.world.unload_map_layer(carla.MapLayer.Decals) + self.world.unload_map_layer(carla.MapLayer.Foliage) + self.world.unload_map_layer(carla.MapLayer.Particles) + self.world.unload_map_layer(carla.MapLayer.Props) + + #################################################################################### + # FOR + # + #################################################################################### + + for episode in tqdm( + range(1, self.env_params.total_episodes + 1), + ascii=True, + unit="episodes", + ): + tensorboard.step = episode + # time.sleep(0.1) + done = False + cumulated_reward = 0 + step = 1 + start_time_epoch = time.time() # datetime.now() + + self.world.tick() + + self.new_car = NewCar( + self.world, + self.environment.environment["start_alternate_pose"], + self.environment.environment["alternate_pose"], + ) + # self.actor_list.append(self.new_car.car) + + ############################################ + ## --------------- Sensors --------------- + ############################################ + + ## --------------- RGB camera --------------- + self.sensor_camera_rgb = CameraRGBSensor(self.new_car.car) + self.actor_list.append(self.sensor_camera_rgb.sensor) + + ## --------------- RedMask Camera --------------- + self.sensor_camera_red_mask = CameraRedMaskSemanticSensor( + self.new_car.car + ) + self.actor_list.append(self.sensor_camera_red_mask.sensor) + + # print( + # f"in main() {self.sensor_camera_rgb.front_rgb_camera} in {time.time()}" + # ) + # print( + # f"in main() {self.sensor_camera_red_mask.front_red_mask_camera} in {time.time()}" + # ) + + ############################################################################ + # ENV, RESET, DQN-AGENT, FOR + ############################################################################ + + env = CarlaEnv( + self.new_car, + self.sensor_camera_red_mask, + self.environment.environment, + ) + # env = gym.make(self.env_params.env_name, **self.environment.environment) + # check_env(env, warn=True) + + self.world.tick() + + ############################ + state, state_size = env.reset() + # print(f"{state = } and {state_size = }") + + # self.world.tick() + + ###################################################################################### + # + # STEPS + ###################################################################################### + + for step in tqdm( + range(1, 100), + ascii=True, + unit="episodes", + ): + # print(f"\n{episode =} , {step = }") + # print(f"{state = } and {state_size = }") + + self.world.tick() + + if np.random.random() > epsilon: + # Get action from Q table + # action = np.argmax(agent_dqn.get_qs(state)) + # print(f"\n{state =}") + # print(f"\n{np.array(state) =}") + # print(f"\n{type(np.array(state)) =}") + # print(f"\n{tf.convert_to_tensor(state) =}") + # print(f"\n{tf.convert_to_tensor(state)[0] =}") + # print(f"\n{tf.convert_to_tensor(state)[:] =}") + + action = np.argmax(dqn_agent.get_qs(state)) + else: + # Get random action + action = np.random.randint( + 0, len(self.global_params.actions_set) + ) + + # print(f"{action = }\n") + + ############################ + + new_state, reward, done, _ = env.step(action) + # print(f"{new_state = } and {reward = } and {done =}") + + # Every step we update replay memory and train main network + # agent_dqn.update_replay_memory((state, action, reward, nextState, done)) + dqn_agent.update_replay_memory( + (state, action, reward, new_state, done) + ) + dqn_agent.train(done, step) + + self.world.tick() + + cumulated_reward += reward + state = new_state + step += 1 + + ######################## + ### solo para chequear que funcione + ######################### + + # if self.sensor_camera_rgb.front_rgb_camera is not None: + # print( + # f"Again in main() self.sensor_camera_rgb.front_rgb_camera in {time.time()}" + # ) + # if self.sensor_camera_red_mask.front_red_mask_camera is not None: + # print( + # f"Again in main() self.sensor_camera_red_mask.front_red_mask_camera in {time.time()}" + # ) + render_params_left_bottom( + episode=episode, + step=step, + observation=state, + new_observation=new_state, + action=action, + v=self.global_params.actions_set[action][ + 0 + ], # this case for discrete + w=self.global_params.actions_set[action][ + 1 + ], # this case for discrete + epsilon=epsilon, + reward_in_step=reward, + cumulated_reward_in_this_episode=cumulated_reward, + _="------------------------", + best_episode_until_now=best_epoch, + in_best_step=best_step, + with_highest_reward=int(current_max_reward), + # in_best_epoch_training_time=best_epoch_training_time, + ) + + AutoCarlaUtils.show_image( + "RGB", + self.sensor_camera_rgb.front_rgb_camera, + 50, + 50, + ) + AutoCarlaUtils.show_image( + "segmentation_cam", + self.sensor_camera_red_mask.front_red_mask_camera, + 500, + 50, + ) + + AutoCarlaUtils.show_image_only_right_line( + "front RGB", + # self.front_rgb_camera[(self.front_rgb_camera.shape[0] // 2) :], + self.sensor_camera_rgb.front_rgb_camera[ + (self.sensor_camera_rgb.front_rgb_camera.shape[0] // 2) : + ], + 1, + env.right_line_in_pixels, + env.ground_truth_pixel_values, + env.dist_normalized, + new_state, + env.x_row, + 1250, + 10, + env.drawing_lines_states, + env.drawing_numbers_states, + ) + + ####################################################### + # + # End render + ####################################################### + + ### reducing epsilon + if epsilon > epsilon_min: + # epsilon *= epsilon_discount + epsilon -= epsilon_decay + + # if episode < 4: + # self.client.apply_batch( + # [carla.command.DestroyActor(x) for x in self.actor_list[::-1]] + # ) + # self.actor_list = [] + + for sensor in self.actor_list: + if sensor.is_listening: + # print(f"is_listening {sensor =}") + sensor.stop() + + # print(f"destroying {sensor =}") + sensor.destroy() + + self.new_car.car.destroy() + self.actor_list = [] + + ############################################################################ + # + # finally + ############################################################################ + + finally: + if self.world is not None: + settings = self.world.get_settings() + settings.synchronous_mode = False + settings.fixed_delta_seconds = None + self.world.apply_settings(settings) + traffic_manager.set_synchronous_mode(True) + print(f"ending training...bye!!") + + # destroy_all_actors() + # for actor in self.actor_list[::-1]: + + # print(f"{self.actor_list =}") + # if len(self.actor_list) + # for actor in self.actor_list: + # actor.destroy() + + # env.close() + + """ + ######################################################### + + ## Reset env + state, state_size = env.reset() + + print_messages( + "main()", + states=self.global_params.states, + states_set=self.global_params.states_set, + states_len=len(self.global_params.states_set), + state_size=state_size, + state=state, + actions=self.global_params.actions, + actions_set=self.global_params.actions_set, + actions_len=len(self.global_params.actions_set), + actions_range=range(len(self.global_params.actions_set)), + batch_size=self.algoritmhs_params.batch_size, + logs_tensorboard_dir=self.global_params.logs_tensorboard_dir, + rewards=self.environment.environment["rewards"], + ) + # Init Agent + dqn_agent = DQN( + self.environment.environment, + self.algoritmhs_params, + len(self.global_params.actions_set), + state_size, + self.global_params.models_dir, + self.global_params, + ) + + # Init TensorBoard + tensorboard = ModifiedTensorBoard( + log_dir=f"{self.global_params.logs_tensorboard_dir}/{self.algoritmhs_params.model_name}-{time.strftime('%Y%m%d-%H%M%S')}" + ) + """ + + """ + for episode in tqdm( + range(1, self.env_params.total_episodes + 1), + ascii=True, + unit="episodes", + ): + tensorboard.step = episode + # time.sleep(0.1) + done = False + cumulated_reward = 0 + step = 1 + + env.world.tick() + start_time_epoch = time.time() # datetime.now() + + observation, _ = env.reset() + while not done: + # print("after") + time_1 = time.time() + env.world.tick() + time_2 = time.time() + # print("before") + time_step = time_2 - time_1 + print(f"{time_step =}") + + if np.random.random() > epsilon: + # Get action from Q table + # action = np.argmax(agent_dqn.get_qs(state)) + action = np.argmax(dqn_agent.get_qs(observation)) + else: + # Get random action + action = np.random.randint( + 0, len(self.global_params.actions_set) + ) + + start_step = time.time() + new_observation, reward, done, _ = env.step(action) + end_step = time.time() + + # Every step we update replay memory and train main network + # agent_dqn.update_replay_memory((state, action, reward, nextState, done)) + dqn_agent.update_replay_memory( + (observation, action, reward, new_observation, done) + ) + dqn_agent.train(done, step) + + self.global_params.time_steps[step] = end_step - start_step + cumulated_reward += reward + observation = new_observation + step += 1 + """ diff --git a/rl_studio/agents/auto_carla/train_followlane_ddpg_carla_tf.py b/rl_studio/agents/auto_carla/train_followlane_ddpg_carla_tf.py new file mode 100755 index 000000000..df3769262 --- /dev/null +++ b/rl_studio/agents/auto_carla/train_followlane_ddpg_carla_tf.py @@ -0,0 +1,544 @@ +from collections import Counter, OrderedDict +from datetime import datetime, timedelta +import glob +from statistics import median +import os +import time + +import carla +import cv2 +import gymnasium as gym +import numpy as np +import pygame +from reloading import reloading +import tensorflow as tf +from tqdm import tqdm + +from rl_studio.agents.f1.loaders import ( + LoadAlgorithmParams, + LoadEnvParams, + LoadEnvVariablesDDPGCarla, + LoadGlobalParams, +) +from rl_studio.agents.utils import ( + render_params, + render_params_left_bottom, + save_dataframe_episodes, + save_carla_dataframe_episodes, + save_batch, + save_best_episode, + LoggingHandler, + print_messages, +) +from rl_studio.agents.utilities.plot_stats import MetricsPlot, StatsDataFrame +from rl_studio.algorithms.ddpg import ( + ModifiedTensorBoard, + OUActionNoise, + Buffer, + DDPGAgent, +) +from rl_studio.algorithms.utils import ( + save_actorcritic_model, +) +from rl_studio.envs.gazebo.gazebo_envs import * +from rl_studio.envs.carla.utils.bounding_boxes import BasicSynchronousClient +from rl_studio.envs.carla.utils.logger import logger +from rl_studio.envs.carla.utils.manual_control import HUD, World +from rl_studio.envs.carla.utils.visualize_multiple_sensors import ( + DisplayManager, + SensorManager, +) +from rl_studio.envs.carla.utils.synchronous_mode import ( + CarlaSyncMode, + draw_image, + get_font, + should_quit, +) +from rl_studio.envs.carla.carla_env import CarlaEnv + +try: + sys.path.append( + glob.glob( + "../carla/dist/carla-*%d.%d-%s.egg" + % ( + sys.version_info.major, + sys.version_info.minor, + "win-amd64" if os.name == "nt" else "linux-x86_64", + ) + )[0] + ) +except IndexError: + pass + + +class TrainerFollowLaneDDPGAutoCarlaTF: + """ + Mode: training + Task: Follow Lane + Algorithm: DDPG + Agent: Auto + Simulator: Carla + Weather: Static + Traffic: No + + The most simplest environment + """ + + def __init__(self, config): + self.algoritmhs_params = LoadAlgorithmParams(config) + self.env_params = LoadEnvParams(config) + self.environment = LoadEnvVariablesDDPGCarla(config) + self.global_params = LoadGlobalParams(config) + + os.makedirs(f"{self.global_params.models_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.logs_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.metrics_data_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.metrics_graphics_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.recorders_carla_dir}", exist_ok=True) + + self.log_file = f"{self.global_params.logs_dir}/{time.strftime('%Y%m%d-%H%M%S')}_{self.global_params.mode}_{self.global_params.task}_{self.global_params.algorithm}_{self.global_params.agent}_{self.global_params.framework}.log" + + # print(f"\nin TrainerFollowLaneQlearnAutoCarla {config=}") + # print(f"\nin TrainerFollowLaneQlearnAutoCarla {self.environment=}\n") + # print( + # f"\nin TrainerFollowLaneQlearnAutoCarla {self.environment.environment=}\n" + # ) + # lanzamos Carla server + CarlaEnv.__init__(self) + + def main(self): + """ + DDPG + """ + log = LoggingHandler(self.log_file) + env = gym.make(self.env_params.env_name, **self.environment.environment) + + random.seed(1) + np.random.seed(1) + tf.compat.v1.random.set_random_seed(1) + + start_time = datetime.now() + best_epoch = 1 + current_max_reward = 0 + best_step = 0 + best_epoch_training_time = 0 + + ## Reset env + state, state_size = env.reset() + + print_messages( + "main()", + states=self.global_params.states, + states_set=self.global_params.states_set, + states_len=len(self.global_params.states_set), + state_size=state_size, + actions=self.global_params.actions, + actions_set=self.global_params.actions_set, + actions_len=len(self.global_params.actions_set), + actions_range=range(len(self.global_params.actions_set)), + batch_size=self.algoritmhs_params.batch_size, + logs_tensorboard_dir=self.global_params.logs_tensorboard_dir, + rewards=self.environment.environment["rewards"], + ) + + ## --------------------- Deep Nets ------------------ + ou_noise = OUActionNoise( + mean=np.zeros(1), + std_deviation=float(self.algoritmhs_params.std_dev) * np.ones(1), + ) + # Init Agents + ac_agent = DDPGAgent( + self.environment.environment, + len(self.global_params.actions_set), + state_size, + self.global_params.models_dir, + ) + # init Buffer + buffer = Buffer( + state_size, + len(self.global_params.actions_set), + self.global_params.states, + self.global_params.actions, + self.algoritmhs_params.buffer_capacity, + self.algoritmhs_params.batch_size, + ) + # Init TensorBoard + tensorboard = ModifiedTensorBoard( + log_dir=f"{self.global_params.logs_tensorboard_dir}/{self.algoritmhs_params.model_name}-{time.strftime('%Y%m%d-%H%M%S')}" + ) + + ## ------------- START TRAINING -------------------- + for episode in tqdm( + range(1, self.env_params.total_episodes + 1), + ascii=True, + unit="episodes", + ): + tensorboard.step = episode + # time.sleep(0.1) + done = False + cumulated_reward = 0 + step = 1 + start_time_epoch = time.time() # datetime.now() + + prev_state, prev_state_size = env.reset() + + while not done: + # if self.environment.environment["sync"]: + env.world.tick() + # else: + # env.world.wait_for_tick() + tf_prev_state = tf.expand_dims(tf.convert_to_tensor(prev_state), 0) + action = ac_agent.policy( + tf_prev_state, ou_noise, self.global_params.actions + ) + start_step = time.time() + state, reward, done, _ = env.step(action) + end_step = time.time() + + self.global_params.time_steps[step] = end_step - start_step + + cumulated_reward += reward + + # learn and update + buffer.record((prev_state, action, reward, state)) + buffer.learn(ac_agent, self.algoritmhs_params.gamma) + ac_agent.update_target( + ac_agent.target_actor.variables, + ac_agent.actor_model.variables, + self.algoritmhs_params.tau, + ) + ac_agent.update_target( + ac_agent.target_critic.variables, + ac_agent.critic_model.variables, + self.algoritmhs_params.tau, + ) + + # + prev_state = state + step += 1 + + """ + print_messages( + "", + episode=episode, + step=step, + action=action, + v=action[0][0], # for continuous actions + w=action[0][1], # for continuous actions + state=state, + reward=reward, + done=done, + current_max_reward=current_max_reward, + ) + """ + # try: + # self.global_params.states_actions_counter[ + # episode, state, action + # ] += 1 + # except KeyError: + # self.global_params.states_actions_counter[ + # episode, state, action + # ] = 1 + + # print_messages( + # "", + # next_state=next_state, + # state=state, + # ) + # env.display_manager.render() + # render params + + render_params_left_bottom( + episode=episode, + step=step, + # action=action, + v=action[0][0], # for continuous actions + w=action[0][1], # for continuous actions + # state=state, + reward_in_step=reward, + cumulated_reward=cumulated_reward, + _="------------------------", + current_max_reward=current_max_reward, + best_epoch=best_epoch, + best_step=best_step, + done=done, + time_per_step=end_step - start_step, + ) + + # best episode and step's stats + if current_max_reward <= cumulated_reward: + current_max_reward = cumulated_reward + best_epoch = episode + best_step = step + best_epoch_training_time = ( + time.time() - start_time_epoch + ) # datetime.now() - start_time_epoch + self.global_params.actions_rewards["episode"].append(episode) + self.global_params.actions_rewards["step"].append(step) + self.global_params.actions_rewards["reward"].append(reward) + # Showing stats in screen for monitoring. Showing every 'save_every_step' value + if not step % self.env_params.save_every_step: + save_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.aggr_ep_rewards, + self.global_params.actions_rewards, + ) + log.logger.info( + f"SHOWING BATCH OF STEPS\n" + f"current_max_reward = {current_max_reward}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"current epoch = {episode}\n" + f"current step = {step}\n" + f"best epoch so far = {best_epoch}\n" + f"best step so far = {best_step}\n" + f"best_epoch_training_time = {best_epoch_training_time}\n" + ) + # print_messages( + # "SHOWING BATCH OF STEPS", + # episode=episode, + # step=step, + # cumulated_reward=cumulated_reward, + # best_epoch=best_epoch, + # best_step=best_step, + # current_max_reward=current_max_reward, + # best_epoch_training_time=best_epoch_training_time, + # ) + + # Reach Finish Line!!! + if env.is_finish: + save_actorcritic_model( + ac_agent, + self.global_params, + self.algoritmhs_params, + self.environment.environment, + cumulated_reward, + episode, + "FINISHLINE", + ) + + print_messages( + "FINISH LINE", + episode=episode, + step=step, + cumulated_reward=cumulated_reward, + ) + log.logger.info( + f"\nFINISH LINE\n" + f"in episode = {episode}\n" + f"steps = {step}\n" + f"cumulated_reward = {cumulated_reward}\n" + ) + + # End epoch + if step > self.env_params.estimated_steps: + done = True + save_actorcritic_model( + ac_agent, + self.global_params, + self.algoritmhs_params, + self.environment.environment, + cumulated_reward, + episode, + "STEPSCOMPLETED", + ) + + print_messages( + "STEPS COMPLETED", + episode=episode, + step=step, + cumulated_reward=cumulated_reward, + ) + log.logger.info( + f"\nSTEPS COMPLETED\n" + f"in episode = {episode}\n" + f"steps = {step}\n" + f"cumulated_reward = {cumulated_reward}\n" + ) + + # check out for Carla Server (end of every step) + ## ----------- checking for Carla Server is working + env.checking_carla_server(self.environment.environment["town"]) + + ######################################## + # collect stats in every epoch + # + ######################################## + + ############ intrinsic + finish_time_epoch = time.time() # datetime.now() + + self.global_params.im_general_ddpg["episode"].append(episode) + self.global_params.im_general_ddpg["step"].append(step) + self.global_params.im_general_ddpg["cumulated_reward"].append( + cumulated_reward + ) + self.global_params.im_general_ddpg["epoch_time"].append( + finish_time_epoch - start_time_epoch + ) + self.global_params.im_general_ddpg["lane_changed"].append( + len(env.lane_changing_hist) + ) + self.global_params.im_general_ddpg["distance_to_finish"].append( + env.dist_to_finish + ) + + # print(f"{self.global_params.time_steps =}") + + ### FPS + fps_m = [values for values in self.global_params.time_steps.values()] + fps_mean = sum(fps_m) / len(self.global_params.time_steps) + + sorted_time_steps = OrderedDict( + sorted(self.global_params.time_steps.items(), key=lambda x: x[1]) + ) + fps_median = median(sorted_time_steps.values()) + # print(f"{fps_mean =}") + # print(f"{fps_median =}") + self.global_params.im_general_ddpg["FPS_avg"].append(fps_mean) + self.global_params.im_general_ddpg["FPS_median"].append(fps_median) + + stats_frame = StatsDataFrame() + stats_frame.save_dataframe_stats( + self.environment.environment, + self.global_params.metrics_graphics_dir, + self.global_params.im_general_ddpg, + ) + + ######################################## + # + # + ######################################## + + # Save best lap + if ( + cumulated_reward - self.environment.environment["rewards"]["penal"] + ) >= current_max_reward: + self.global_params.best_current_epoch["best_epoch"].append(best_epoch) + self.global_params.best_current_epoch["highest_reward"].append( + cumulated_reward + ) + self.global_params.best_current_epoch["best_step"].append(best_step) + self.global_params.best_current_epoch[ + "best_epoch_training_time" + ].append(best_epoch_training_time) + self.global_params.best_current_epoch[ + "current_total_training_time" + ].append(datetime.now() - start_time) + save_carla_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.best_current_epoch, + ) + + save_actorcritic_model( + ac_agent, + self.global_params, + self.algoritmhs_params, + self.environment.environment, + cumulated_reward, + episode, + "BESTLAP", + ) + + log.logger.info( + f"\nsaving best lap\n" + f"in episode = {episode}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"current_max_reward = {current_max_reward}\n" + f"steps = {step}\n" + ) + + # end of training by: + # training time setting: 2 hours, 15 hours... + # num epochs + + if ( + datetime.now() - timedelta(hours=self.global_params.training_time) + > start_time + ) or (episode > self.env_params.total_episodes): + if ( + cumulated_reward - self.environment.environment["rewards"]["penal"] + ) >= current_max_reward: + save_actorcritic_model( + ac_agent, + self.global_params, + self.algoritmhs_params, + self.environment.environment, + cumulated_reward, + episode, + "FINISHTIME", + ) + log.logger.info( + f"\nTraining Time over\n" + f"current_max_reward = {current_max_reward}\n" + f"epoch = {episode}\n" + f"step = {step}\n" + ) + + break + + ##################################################### + # save best values every save_episode times + self.global_params.ep_rewards.append(cumulated_reward) + if not episode % self.env_params.save_episodes: + average_reward = sum( + self.global_params.ep_rewards[-self.env_params.save_episodes :] + ) / len(self.global_params.ep_rewards[-self.env_params.save_episodes :]) + min_reward = min( + self.global_params.ep_rewards[-self.env_params.save_episodes :] + ) + max_reward = max( + self.global_params.ep_rewards[-self.env_params.save_episodes :] + ) + tensorboard.update_stats( + reward_avg=int(average_reward), + reward_max=int(max_reward), + steps=step, + ) + + self.global_params.aggr_ep_rewards = save_batch( + episode, + step, + start_time_epoch, + start_time, + self.global_params, + self.env_params, + ) + save_carla_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.aggr_ep_rewards, + ) + + if max_reward > current_max_reward: + save_actorcritic_model( + ac_agent, + self.global_params, + self.algoritmhs_params, + self.environment.environment, + cumulated_reward, + episode, + "BATCH", + ) + + ## ------------ destroy actors + env.destroy_all_actors() + # ----------- end for + + """ + ### States, Actions counter in every epoch + for key, value in self.global_params.states_actions_counter.items(): + self.global_params.im_state_actions_counter["epoch"].append(key[0]) + self.global_params.im_state_actions_counter["state"].append(key[1]) + self.global_params.im_state_actions_counter["action"].append(key[2]) + self.global_params.im_state_actions_counter["counter"].append(value) + stats_frame.save_dataframe_stats( + self.environment.environment, + self.global_params.metrics_graphics_dir, + self.global_params.im_state_actions_counter, + sac=True, + ) + """ + + env.close() diff --git a/rl_studio/agents/auto_carla/train_followlane_dqn_carla_tf.py b/rl_studio/agents/auto_carla/train_followlane_dqn_carla_tf.py new file mode 100644 index 000000000..3f4fe03ac --- /dev/null +++ b/rl_studio/agents/auto_carla/train_followlane_dqn_carla_tf.py @@ -0,0 +1,879 @@ +from collections import Counter, OrderedDict, deque +from datetime import datetime, timedelta +import gc +import glob + +# import math +from memory_profiler import memory_usage # , profile + +# import resource +import subprocess +from statistics import median +import os + +os.environ["TF_CPP_MIN_LOG_LEVEL"] = "3" +import random +import sys +import time + +import carla + +# import keras +import numpy as np +import tensorflow as tf +from tqdm import tqdm + +# from rl_studio.agents.auto_carla.actors_sensors import ( +# NewCar, +# CameraRGBSensor, +# CameraRedMaskSemanticSensor, +# # LaneDetector, +# ) +from rl_studio.agents.auto_carla.carla_env import CarlaEnv +from rl_studio.agents.f1.loaders import ( + LoadAlgorithmParams, + LoadEnvParams, + LoadGlobalParams, + LoadEnvVariablesDQNCarla, +) +from rl_studio.agents.auto_carla.utils import ( + # LoggingHandler, + Logger, + # LoggerAllInOne, + format_time, + get_variables_size, +) +from rl_studio.agents.utils import ( + render_params, + render_params_left_bottom, + save_dataframe_episodes, + save_carla_dataframe_episodes, + save_batch, + save_best_episode, + # LoggingHandler, + print_messages, +) +from rl_studio.agents.utilities.plot_stats import MetricsPlot, StatsDataFrame + +# from rl_studio.algorithms.ddpg import ( +# ModifiedTensorBoard, +# ) +from rl_studio.algorithms.dqn_keras import ( + ModifiedTensorBoard, + DQN, +) + +try: + sys.path.append( + glob.glob( + "../carla/dist/carla-*%d.%d-%s.egg" + % ( + sys.version_info.major, + sys.version_info.minor, + "win-amd64" if os.name == "nt" else "linux-x86_64", + ) + )[0] + ) +except IndexError: + pass + + +################################################################################## +# +# GPUs management +################################################################################## + +tf.debugging.set_log_device_placement(False) + +# Sharing GPU +gpus = tf.config.experimental.list_physical_devices("GPU") + +for gpu in gpus: + tf.config.experimental.set_memory_growth(gpu, True) # auto memory configuration + +logical_gpus = tf.config.list_logical_devices("GPU") +print( + f"\n\tIn train_followlane_dqn_carla_tf.py ---> {len(gpus)} Physical GPUs, {len(logical_gpus)} Logical GPUs" +) + +############ 1 phisical GPU + 2 logial GPUs +# gpus = tf.config.list_physical_devices("GPU") +# if gpus: +# # Create 2 virtual GPUs with 1GB memory each +# try: +# tf.config.set_logical_device_configuration( +# gpus[0], +# [ +# tf.config.LogicalDeviceConfiguration(memory_limit=1024), +# tf.config.LogicalDeviceConfiguration(memory_limit=1024), +# tf.config.LogicalDeviceConfiguration(memory_limit=1024), +# tf.config.LogicalDeviceConfiguration(memory_limit=1024), +# ], +# ) +# logical_gpus = tf.config.list_logical_devices("GPU") +# print(len(gpus), "Physical GPU,", len(logical_gpus), "Logical GPUs") +# except RuntimeError as e: +# # Virtual devices must be set before GPUs have been initialized +# print(e) + + +# Init Ray +# ray.init(ignore_reinit_error=True) + + +############################################################################################# +# +# Trainer +############################################################################################# + + +class TrainerFollowLaneDQNAutoCarlaTF: + """ + + Mode: training + Task: Follow Lane + Algorithm: DQN + Agent: Auto + Simulator: Carla + Framework: TF + Weather: Static + Traffic: No + + The most simplest environment + """ + + def __init__(self, config): + + self.algoritmhs_params = LoadAlgorithmParams(config) + self.env_params = LoadEnvParams(config) + self.environment = LoadEnvVariablesDQNCarla(config) + self.global_params = LoadGlobalParams(config) + + os.makedirs(f"{self.global_params.models_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.logs_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.metrics_data_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.metrics_graphics_dir}", exist_ok=True) + os.makedirs(f"{self.global_params.recorders_carla_dir}", exist_ok=True) + + self.log_file = f"{self.global_params.logs_dir}/{time.strftime('%Y%m%d-%H%M%S')}_{self.global_params.mode}_{self.global_params.task}_{self.global_params.algorithm}_{self.global_params.agent}_{self.global_params.framework}.log" + + # CarlaEnv.__init__(self) + + self.world = None + self.client = None + self.front_rgb_camera = None + self.front_red_mask_camera = None + self.front_lanedetector_camera = None + self.actor_list = [] + + def launch_carla_server(self): + ### Launch Carla Server only if it is not running!!! + ps_output = subprocess.check_output(["ps", "-Af"]).decode("utf-8").strip("\n") + + # CHECKING IF CARLA SERVER IS RUNNING + if ps_output.count("CarlaUE4-Linux-") > 0: + try: + subprocess.check_call(["killall", "-9", "CarlaUE4-Linux-"]) + except subprocess.CalledProcessError as ce: + print( + "SimulatorEnv: exception raised executing killall command for CarlaUE4-Linux- {}".format( + ce + ) + ) + if ps_output.count("CarlaUE4.sh") > 0: + try: + subprocess.check_call(["killall", "-9", "CarlaUE4.sh"]) + except subprocess.CalledProcessError as ce: + print( + "SimulatorEnv: exception raised executing killall command for CARLA server {}".format( + ce + ) + ) + + if ps_output.count("CarlaUE4-Linux-Shipping") > 0: + try: + subprocess.check_call(["killall", "-9", "CarlaUE4-Linux-Shipping"]) + except subprocess.CalledProcessError as ce: + print( + "SimulatorEnv: exception raised executing killall command for CarlaUE4-Linux-Shipping {}".format( + ce + ) + ) + + if ( + ps_output.count("CarlaUE4-Linux-") == 0 + or ps_output.count("CarlaUE4.sh") == 0 + or ps_output.count("CarlaUE4-Linux-Shipping") == 0 + ): + try: + carla_root_local = os.environ["CARLA_ROOT"] + carla_exec_local = ( + f"{carla_root_local}/CarlaUE4.sh -prefernvidia -quality-level=low" + ) + + carla_root_landau = f"/opt/carla/CarlaUE4.sh -world-port=4545" + # carla_exec_landau = f"{carla_root_landau}/CarlaUE4.sh -prefernvidia -quality-level=low" + + # with open("/tmp/.carlalaunch_stdout.log", "w") as out, open( + # "/tmp/.carlalaunch_stderr.log", "w" + # ) as err: + # subprocess.Popen( + # [carla_exec, "-prefernvidia -quality-level=low"], + # stdout=out, + # stderr=err, + # ) + subprocess.Popen( + ["gnome-terminal", "--", "bash", "-c", carla_root_landau] + ) + + # subprocess.Popen([carla_exec, "-prefernvidia"], stdout=out, stderr=err) + # subprocess.Popen(["/home/jderobot/Documents/Projects/carla_simulator_0_9_13/CarlaUE4.sh", "-RenderOffScreen"], stdout=out, stderr=err) + # subprocess.Popen(["/home/jderobot/Documents/Projects/carla_simulator_0_9_13/CarlaUE4.sh", "-RenderOffScreen", "-quality-level=Low"], stdout=out, stderr=err) + print(f"\nCarlaEnv has been launched in other terminal\n") + time.sleep(5) + # with open("/tmp/.roslaunch_stdout.log", "w") as out, open("/tmp/.roslaunch_stderr.log", "w") as err: + # child = subprocess.Popen(["roslaunch", launch_file], stdout=out, stderr=err) + # logger.info("SimulatorEnv: launching simulator server.") + except OSError as oe: + print( + "SimulatorEnv: exception raised launching simulator server. {}".format( + oe + ) + ) + + def closing_carla_server(self, log): + + try: + ps_output = ( + subprocess.check_output(["ps", "-Af"]).decode("utf-8").strip("\n") + ) + except subprocess.CalledProcessError as ce: + log._warning( + "SimulatorEnv: exception raised executing ps command {}".format(ce) + ) + sys.exit(-1) + + if ps_output.count("CarlaUE4-Linux-") > 0: + try: + subprocess.check_call(["killall", "-9", "CarlaUE4-Linux-"]) + log._warning("SimulatorEnv: CarlaUE4-Linux- killed.") + except subprocess.CalledProcessError as ce: + log._warning( + "SimulatorEnv: exception raised executing killall command for CarlaUE4-Linux- {}".format( + ce + ) + ) + if ps_output.count("CarlaUE4.sh") > 0: + try: + subprocess.check_call(["killall", "-9", "CarlaUE4.sh"]) + log._warning("SimulatorEnv: CarlaUE4.sh killed.") + except subprocess.CalledProcessError as ce: + log._warning( + "SimulatorEnv: exception raised executing killall command for CARLA server {}".format( + ce + ) + ) + + if ps_output.count("CarlaUE4-Linux-Shipping") > 0: + try: + subprocess.check_call(["killall", "-9", "CarlaUE4-Linux-Shipping"]) + log._warning("SimulatorEnv: CarlaUE4-Linux-Shipping killed.") + except subprocess.CalledProcessError as ce: + log._warning( + "SimulatorEnv: exception raised executing killall command for CarlaUE4-Linux-Shipping {}".format( + ce + ) + ) + + print( + f"\n\tclosing env", + f"\n\tKilled Carla server", + f"\n\tending training...have a good day!!", + ) + + ######################################################################### + # Main + ######################################################################### + + def main(self): + """ """ + ######################################################################### + # Vars + ######################################################################### + + # log = LoggingHandler(self.log_file) + # log = LoggerAllInOne(self.log_file) + log = Logger(self.log_file) + random.seed(1) + np.random.seed(1) + tf.compat.v1.random.set_random_seed(1) + + start_time = datetime.now() + best_epoch = 1 + current_max_reward = 0 + best_step = 0 + best_epoch_training_time = 0 + epsilon = self.algoritmhs_params.epsilon + epsilon_discount = self.algoritmhs_params.epsilon_discount + epsilon_min = self.algoritmhs_params.epsilon_min + epsilon_decay = epsilon / (self.env_params.total_episodes // 2) + + ### DQN declaration + ### --- SIMPLIFIED PERCEPTION: + ### DQN size: [centers_n, line_borders_n * 2, v, w, angle] + ### DQN size = (x_row * 3) + 3 + ### ---- IMAGE + + if self.global_params.states == "image": + DQN_size = ( + self.environment.environment["new_image_size"], + self.environment.environment["new_image_size"], + 1, + ) + else: + DQN_size = (len(self.environment.environment["x_row"]) * 3) + 3 + + # input(f"{DQN_size =}") + + dqn_agent = DQN( + self.environment.environment, + self.algoritmhs_params, + len(self.global_params.actions_set), + # len(self.environment.environment["x_row"]), + DQN_size, + self.global_params.models_dir, + self.global_params, + ) + # Init TensorBoard + tensorboard = ModifiedTensorBoard( + log_dir=f"{self.global_params.logs_tensorboard_dir}/{self.algoritmhs_params.model_name}-{time.strftime('%Y%m%d-%H%M%S')}" + ) + + ### Launch Carla Server only if it is not running!!! + if self.global_params.station == "landau_V2": + self.launch_carla_server() + + ################################ + # + # TRAINING + ############################################ + + try: + ######################################################################### + # Vars + ######################################################################### + # print(f"\n al inicio de Main() {self.environment.environment =}\n") + self.client = carla.Client( + self.environment.environment["carla_server"], + self.environment.environment["carla_client"], + ) + self.client.set_timeout(3.0) + print( + f"\n In TrainerFollowLaneDQNAutoCarlaTF/main() ---> maps in carla 0.9.13: {self.client.get_available_maps()}\n" + ) + + self.world = self.client.load_world(self.environment.environment["town"]) + + # Sync mode + traffic_manager = self.client.get_trafficmanager( + self.environment.environment["traffic_manager_port"] + ) + settings = self.world.get_settings() + settings.synchronous_mode = True + traffic_manager.set_synchronous_mode(True) + + settings.fixed_delta_seconds = 0.1 # read: https://carla.readthedocs.io/en/0.9.13/adv_synchrony_timestep/# Phisics substepping + # With 0.05 value, the simulator will take twenty steps (1/0.05) to recreate one second of the simulated world + self.world.apply_settings(settings) + + # Set up the traffic manager + # traffic_manager = self.client.get_trafficmanager(8000) + # traffic_manager.set_synchronous_mode(True) + # traffic_manager.set_random_device_seed(0) # define TM seed for determinism + + self.client.reload_world(False) + + # print(f"{settings.synchronous_mode =}") + # self.world.tick() + # print(f"{self.world.tick()=}") + + # Town07 take layers off + if self.environment.environment["town"] == "Town07_Opt": + self.world.unload_map_layer(carla.MapLayer.Buildings) + self.world.unload_map_layer(carla.MapLayer.Decals) + self.world.unload_map_layer(carla.MapLayer.Foliage) + self.world.unload_map_layer(carla.MapLayer.Particles) + self.world.unload_map_layer(carla.MapLayer.Props) + + ## LaneDetector Camera --------------- + # self.sensor_camera_lanedetector = LaneDetector( + # "models/fastai_torch_lane_detector_model.pth" + # ) + + env = CarlaEnv( + # self.new_car, + # self.sensor_camera_rgb, # img to process + # self.sensor_camera_red_mask, # sensor to process image + self.client, + self.world, + self.environment.environment, + ) + self.world.tick() + + #################################################################################### + # FOR + # + #################################################################################### + memory_use_before_1_epoch = memory_usage()[0] + + for episode in tqdm( + range(1, self.env_params.total_episodes + 1), + ascii=True, + unit="episodes", + ): + memory_use_after_FOR = memory_usage()[0] + tensorboard.step = episode + # time.sleep(0.1) + done = False + cumulated_reward = 0 + step = 1 + start_time_epoch = time.time() # datetime.now() + + # self.world.tick() + + state = env.reset() + # print(f"\n\tin Training For loop -------> {state =}") + ###################################################################################### + # + # STEPS + ###################################################################################### + + while not done: + # print(f"\n{episode =} , {step = }") + # print(f"{state = } and {state_size = }") + + self.world.tick() + + memory_use_after_while = memory_usage()[0] + start_training_step = time.time() + + """ + # epsilon = 0.1 ###### PARA PROBAR-----------OJO QUITARLO + if np.random.random() > epsilon: + # print(f"\n\tin Training For loop -----> {epsilon =}") + action = np.argmax(dqn_agent.get_qs(state)) + else: + # Get random action + action = np.random.randint( + 0, len(self.global_params.actions_set) + ) + """ + + action = dqn_agent.choose_action(state, epsilon) + + memory_use_after_ACTION = memory_usage()[0] + # print(f"{action = }\n") + + ############################ + start_training_step = time.time() + start_step = time.time() + + new_state, reward, done, _ = env.step(action) + # print(f"{new_state = } and {reward = } and {done =}") + # print( + # f"\n\tin Training For loop ---> {state =}, {action =}, {new_state =}, {reward =}, {done =}" + # ) + + # Every step we update replay memory and train main network + # agent_dqn.update_replay_memory((state, action, reward, nextState, done)) + end_step: float = time.time() + memory_use_after_STEP = memory_usage()[0] + + dqn_agent.update_replay_memory( + (state, action, reward, new_state, done) + ) + dqn_agent.train(done, step) + + memory_use_after_every_NN_train = memory_usage()[0] + end_training_step = time.time() + + # self.world.tick() + + self.global_params.time_steps[step] = end_step - start_step + self.global_params.time_training_steps[step] = ( + end_training_step - start_training_step + ) + + cumulated_reward += reward + state = new_state + step += 1 + + render_params_left_bottom( + episode=episode, + step=step, + epsilon=epsilon, + # observation=state, + # new_observation=new_state, + action=action, + throttle=self.global_params.actions_set[action][ + 0 + ], # this case for discrete + steer=self.global_params.actions_set[action][ + 1 + ], # this case for discrete + v_km_h=env.params["current_speed"], + w_deg_sec=env.params["current_steering_angle"], + angle=env.angle, + FPS=1 / (end_step - start_step), + FPS_training_step=1 / (end_training_step - start_training_step), + centers=sum(env.centers_normal) / len(env.centers_normal), + reward_in_step=reward, + cumulated_reward_in_this_episode=cumulated_reward, + memory_use_before_1_epoch=memory_use_before_1_epoch, + memory_use_after_FOR=memory_use_after_FOR, + memory_use_after_while=memory_use_after_while, + memory_use_after_ACTION=memory_use_after_ACTION, + memory_use_after_STEP=memory_use_after_STEP, + memory_use_after_every_NN_train=memory_use_after_every_NN_train, + memory_use_in_every_step=memory_use_after_every_NN_train + - memory_use_after_while, + memory_use_in_every_for=memory_use_after_every_NN_train + - memory_use_after_FOR, + _="------------------------", + best_episode_until_now=best_epoch, + in_best_step=best_step, + with_highest_reward=int(current_max_reward), + # in_best_epoch_training_time=best_epoch_training_time, + ) + + # best episode + if current_max_reward <= cumulated_reward and episode > 1: + current_max_reward = cumulated_reward + best_epoch = episode + best_step = step + best_epoch_training_time = time.time() - start_time_epoch + self.global_params.actions_rewards["episode"].append(episode) + self.global_params.actions_rewards["step"].append(step) + self.global_params.actions_rewards["reward"].append(reward) + + # Showing stats in screen for monitoring. Showing every 'save_every_step' value + if not step % self.env_params.save_every_step: + save_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.aggr_ep_rewards, + self.global_params.actions_rewards, + ) + log._warning( + f"SHOWING BATCH OF STEPS\n" + f"current epoch = {episode}\n" + f"current step = {step}\n" + f"epsilon = {epsilon}\n" + f"current_max_reward = {current_max_reward}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"best epoch so far = {best_epoch}\n" + f"best step so far = {best_step}\n" + f"best_epoch_training_time = {best_epoch_training_time}\n" + ) + + # Reach Finish Line!!! + if env.is_finish: + dqn_agent.model.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_CIRCUITCOMPLETED_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.keras", + ) + dqn_agent.model.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_CIRCUITCOMPLETED_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.h5", + ) + dqn_agent.model.save_weights( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_CIRCUITCOMPLETED_WEIGHTS_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.h5", + ) + print_messages( + "FINISH LINE", + episode=episode, + step=step, + cumulated_reward=cumulated_reward, + ) + log._warning( + f"\nFINISH LINE\n" + f"in episode = {episode}\n" + f"steps = {step}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"epsilon = {epsilon}\n" + ) + ### save in case of completed steps in one episode + if step >= self.env_params.estimated_steps: + done = True + dqn_agent.model.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_STEPSCOMPLETED_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.keras", + ) + dqn_agent.model.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_STEPSCOMPLETED_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.h5", + ) + dqn_agent.model.save_weights( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_STEPSCOMPLETED_WEIGHTS_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.h5", + ) + log._warning( + f"\nEPISODE COMPLETED\n" + f"in episode = {episode}\n" + f"steps = {step}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"epsilon = {epsilon}\n" + ) + + ######################################## + # collect stats in every epoch + # + ######################################## + + ############ intrinsic + finish_time_epoch = time.time() # datetime.now() + + self.global_params.im_general_ddpg["episode"].append(episode) + self.global_params.im_general_ddpg["step"].append(step) + self.global_params.im_general_ddpg["cumulated_reward"].append( + cumulated_reward + ) + self.global_params.im_general_ddpg["epoch_time"].append( + finish_time_epoch - start_time_epoch + ) + self.global_params.im_general_ddpg["lane_changed"].append( + len(env.lane_changing_hist) + ) + self.global_params.im_general_ddpg["distance_to_finish"].append( + env.dist_to_finish + ) + + # print(f"{self.global_params.time_steps =}") + + ### FPS + fps_m = [values for values in self.global_params.time_steps.values()] + fps_mean = sum(fps_m) / len(self.global_params.time_steps) + + sorted_time_steps = OrderedDict( + sorted(self.global_params.time_steps.items(), key=lambda x: x[1]) + ) + fps_median = median(sorted_time_steps.values()) + # print(f"{fps_mean =}") + # print(f"{fps_median =}") + self.global_params.im_general_ddpg["FPS_avg"].append(fps_mean) + self.global_params.im_general_ddpg["FPS_median"].append(fps_median) + + stats_frame = StatsDataFrame() + stats_frame.save_dataframe_stats( + self.environment.environment, + self.global_params.metrics_graphics_dir, + self.global_params.im_general_ddpg, + ) + + ######################################## + # + # + ######################################## + #### save best lap in episode + if ( + cumulated_reward - self.environment.environment["rewards"]["penal"] + ) >= current_max_reward and episode > 1: + self.global_params.best_current_epoch["best_epoch"].append( + best_epoch + ) + self.global_params.best_current_epoch["highest_reward"].append( + current_max_reward + ) + self.global_params.best_current_epoch["best_step"].append(best_step) + self.global_params.best_current_epoch[ + "best_epoch_training_time" + ].append(best_epoch_training_time) + self.global_params.best_current_epoch[ + "current_total_training_time" + ].append(datetime.now() - start_time) + + save_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.best_current_epoch, + ) + dqn_agent.model.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_BESTLAP_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.keras", + ) + dqn_agent.model.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_BESTLAP_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.h5", + ) + dqn_agent.model.save_weights( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_BESTLAP_WEIGHTS_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.h5", + ) + log._warning( + f"\nSAVING BEST LAP\n" + f"in episode = {episode}\n" + f"steps = {step}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"current_max_reward = {current_max_reward}\n" + f"epsilon = {epsilon}\n" + ) + # end episode in time settings: 2 hours, 15 hours... + # or epochs over + if ( + datetime.now() - timedelta(hours=self.global_params.training_time) + > start_time + ) or (episode > self.env_params.total_episodes): + log._warning( + f"\nTraining Time over or num epochs reached\n" + f"epoch = {episode}\n" + f"step = {step}\n" + f"current_max_reward = {current_max_reward}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"epsilon = {epsilon}\n" + ) + if ( + cumulated_reward + - self.environment.environment["rewards"]["penal"] + ) >= current_max_reward: + dqn_agent.model.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_LAPCOMPLETED_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.keras", + ) + dqn_agent.model.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_LAPCOMPLETED_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.h5", + ) + dqn_agent.model.save_weights( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_LAPCOMPLETED_WEIGHTS_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.h5", + ) + + break + + ### save every save_episode times + self.global_params.ep_rewards.append(cumulated_reward) + if not episode % self.env_params.save_episodes: + + # input("parada en Training al saving el batch.....verificar valores") + average_reward = sum( + self.global_params.ep_rewards[-self.env_params.save_episodes :] + ) / len( + self.global_params.ep_rewards[-self.env_params.save_episodes :] + ) + min_reward = min( + self.global_params.ep_rewards[-self.env_params.save_episodes :] + ) + max_reward = max( + self.global_params.ep_rewards[-self.env_params.save_episodes :] + ) + tensorboard.update_stats( + reward_avg=int(average_reward), + reward_max=int(max_reward), + steps=step, + epsilon=epsilon, + ) + + self.global_params.aggr_ep_rewards["episode"].append(episode) + self.global_params.aggr_ep_rewards["step"].append(step) + self.global_params.aggr_ep_rewards["avg"].append(average_reward) + self.global_params.aggr_ep_rewards["max"].append(max_reward) + self.global_params.aggr_ep_rewards["min"].append(min_reward) + self.global_params.aggr_ep_rewards["epoch_training_time"].append( + (time.time() - start_time_epoch) + ) + self.global_params.aggr_ep_rewards["total_training_time"].append( + (datetime.now() - start_time).total_seconds() + ) + if max_reward > current_max_reward: + dqn_agent.model.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_BATCH_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.keras", + ) + dqn_agent.model.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_BATCH_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.h5", + ) + dqn_agent.model.save_weights( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['circuit_name']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_BATCH_WEIGHTS_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}_{self.algoritmhs_params.model_name}_SERVER{self.environment.environment['carla_server']}_CLIENT{self.environment.environment['carla_client']}.h5", + ) + save_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.aggr_ep_rewards, + ) + log._warning( + f"\nsaving BATCH\n" + f"best_epoch = {best_epoch}\n" + f"best_step = {best_step}\n" + f"current_max_reward = {current_max_reward}\n" + f"best_epoch_training_time = {best_epoch_training_time}\n" + f"epsilon = {epsilon}\n" + ) + ####################################################### + # + # End FOR + ####################################################### + + ### reducing epsilon + if epsilon > epsilon_min: + # epsilon *= epsilon_discount + epsilon -= epsilon_decay + + # for actor in env.actor_list[::-1]: + # print(f"Destroying {actor}") + # actor.destroy() + + env.destroy_all_actors() + # env.actor_list = [] + + variables_size, total_size = get_variables_size() + for var_name, var_value in variables_size.items(): + size = sys.getsizeof(var_value) + log._warning(f"\n{var_name}: {size} bytes |" f"\t{total_size =}") + + del sorted_time_steps + gc.collect() + + ## VERIFY DATA + show = 10 + if ( + done + and (not episode % show) + and self.global_params.station != "landau" + ): + input( + f"\n\t{env.center_reward =}, {env.done_center =}, {env.centers_normal =}" + f"\n\t{env.velocity_reward =}, {env.done_velocity =}" + f"\n\t{env.heading_reward =}, {env.done_heading =}" + f"\n\t{done =}, {reward =}" + f"\n\t{env.params['current_speed'] =}, {env.params['target_veloc'] =}, {env.angle =}" + f"\n\t{env.params['current_steering_angle'] =}" + f"\n\t{state}" + ) + ### save last episode, not neccesarily the best one + save_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.aggr_ep_rewards, + ) + + ############################################################################ + # + # finally + ############################################################################ + + finally: + if self.world is not None: + settings = self.world.get_settings() + settings.synchronous_mode = False + traffic_manager.set_synchronous_mode(False) + settings.fixed_delta_seconds = None + self.world.apply_settings(settings) + + # destroy_all_actors() + # for actor in self.actor_list[::-1]: + + # print(f"{self.actor_list =}") + # if len(self.actor_list) + # for actor in self.actor_list: + # actor.destroy() + + env.close() + + if self.global_params.station == "landau": + ps_output = ( + subprocess.check_output(["ps", "-Af"]).decode("utf-8").strip("\n") + ) + # If there are NOT python scripts running....kill the server + if ps_output.count("python") < 1: + self.closing_carla_server(log) + + ## kill python script in case othersPython script being running + sys.exit(0) diff --git a/rl_studio/agents/auto_carla/train_followlane_qlearn_carla.py b/rl_studio/agents/auto_carla/train_followlane_qlearn_carla.py index 945c50b91..0554e3da9 100755 --- a/rl_studio/agents/auto_carla/train_followlane_qlearn_carla.py +++ b/rl_studio/agents/auto_carla/train_followlane_qlearn_carla.py @@ -1,5 +1,7 @@ +from collections import Counter, OrderedDict from datetime import datetime, timedelta import glob +from statistics import median import os import time @@ -19,13 +21,16 @@ ) from rl_studio.agents.utils import ( render_params, + render_params_left_bottom, save_dataframe_episodes, + save_carla_dataframe_episodes, save_batch, save_best_episode, LoggingHandler, print_messages, ) -from rl_studio.algorithms.qlearn import QLearnCarla +from rl_studio.agents.utilities.plot_stats import MetricsPlot, StatsDataFrame +from rl_studio.algorithms.qlearn import QLearnCarla, QLearn from rl_studio.envs.gazebo.gazebo_envs import * from rl_studio.envs.carla.utils.bounding_boxes import BasicSynchronousClient from rl_studio.envs.carla.utils.logger import logger @@ -61,7 +66,7 @@ class TrainerFollowLaneQlearnAutoCarla: """ Mode: training Task: Follow Lane - Algorithm: Qlearn + Algorithm: QlearnCarla Agent: Auto Simulator: Carla Weather: Static @@ -93,9 +98,17 @@ def __init__(self, config): CarlaEnv.__init__(self) def main(self): + """ + Qlearn dictionnary + """ log = LoggingHandler(self.log_file) env = gym.make(self.env_params.env_name, **self.environment.environment) + start_time = datetime.now() + best_epoch = 1 + current_max_reward = 0 + best_step = 0 + best_epoch_training_time = 0 epsilon = self.environment.environment["epsilon"] epsilon_decay = epsilon / (self.env_params.total_episodes) # epsilon = epsilon / 2 @@ -113,14 +126,14 @@ def main(self): "main()", actions_len=len(self.global_params.actions_set), actions_range=range(len(self.global_params.actions_set)), - actions=self.global_params.actions_set, + actions_set=self.global_params.actions_set, epsilon=epsilon, epsilon_decay=epsilon_decay, alpha=self.environment.environment["alpha"], gamma=self.environment.environment["gamma"], ) ## --- init Qlearn - qlearn = QLearnCarla( + qlearn = QLearn( actions=range(len(self.global_params.actions_set)), epsilon=self.environment.environment["epsilon"], alpha=self.environment.environment["alpha"], @@ -129,46 +142,57 @@ def main(self): # log.logger.info( # f"\nqlearn.q_table = {qlearn.q_table}", # ) - print_messages( - "", - qlearn_q_table=qlearn.q_table, - ) - ## ------------- START TRAINING -------------------- + # print_messages( + # "", + # qlearn_q_table=qlearn.q_table, + # ) + ## retraining q model + if self.environment.environment["mode"] == "retraining": + qlearn.load_pickle_model( + f"{self.global_params.models_dir}/{self.environment.environment['retrain_qlearn_model_name']}" + ) + # using epsilon reduced + epsilon = epsilon / 10 + print(f"{qlearn.q = }") + + ## ------------- START TRAINING -------------------- for episode in tqdm( range(1, self.env_params.total_episodes + 1), ascii=True, unit="episodes", ): - + time.sleep(0.1) done = False cumulated_reward = 0 step = 0 + start_time_epoch = time.time() # datetime.now() observation = env.reset() state = "".join(map(str, observation)) - print_messages( - "in episode", - episode=episode, - observation=observation, - state=state, - type_observation=type(observation), - type_state=type(state), - ) while not done: - if self.environment.environment["sync"]: - env.world.tick() - else: - env.world.wait_for_tick() + # if self.environment.environment["sync"]: + env.world.tick() + # else: + # env.world.wait_for_tick() step += 1 - action = qlearn.select_action(state) + start_step = time.time() + + action = qlearn.selectAction(state) # print(f"{action = }") - new_observation, reward, done, _ = env.step(action) + observation, reward, done, _ = env.step(action) + # time.sleep(4) + # print(f"\n{end_step - start_step = }") cumulated_reward += reward - next_state = "".join(map(str, new_observation)) + next_state = "".join(map(str, observation)) + end_step = time.time() + self.global_params.time_steps[step] = end_step - start_step + + """ print_messages( "", + episode=episode, step=step, action=action, state=state, @@ -176,378 +200,761 @@ def main(self): reward=reward, done=done, next_state=next_state, - # state=state, + current_max_reward=current_max_reward, ) + """ + """ + log.logger.info( + f"\n step = {step}\n" + f"action = {action}\n" + f"actions type = {type(action)}\n" + f"state = {state}\n" + # f"observation[0]= {observation[0]}\n" + # f"observation type = {type(observation)}\n" + # f"observation[0] type = {type(observation[0])}\n" + f"new_observation = {new_observation}\n" + f"type new_observation = {type(new_observation)}\n" + f"reward = {reward}\n" + f"done = {done}\n" + f"next_state = {next_state}\n" + f"type new_observation = {type(new_observation)}\n" + f"current_max_reward = {current_max_reward}\n" + ) + """ qlearn.learn(state, action, reward, next_state) state = next_state - print_messages( - "", - next_state=next_state, - state=state, - ) - env.display_manager.render() - ## ------------ destroy actors - - env.destroy_all_actors() - env.display_manager.destroy() - # ----------- end for + try: + self.global_params.states_actions_counter[ + episode, state, action + ] += 1 + except KeyError: + self.global_params.states_actions_counter[ + episode, state, action + ] = 1 + + # print_messages( + # "", + # next_state=next_state, + # state=state, + # ) + # env.display_manager.render() + # render params - env.close() + render_params_left_bottom( + episode=episode, + step=step, + action=action, + epsilon=epsilon, + reward_in_step=reward, + cumulated_reward=cumulated_reward, + _="------------------------", + current_max_reward=current_max_reward, + best_epoch=best_epoch, + best_step=best_step, + done=done, + time_per_step=end_step - start_step, + FPS=1/(end_step - start_step), + ) - ###################################################################################################### - def AAAAmain(self): - """ - esta funcionando, muestra la imagen en Pygame pero no se actualiza en el step - """ - # print(f"\nself.env_params.env_name = {self.env_params.env_name}\n") + # best episode and step's stats + if current_max_reward <= cumulated_reward: + current_max_reward = cumulated_reward + best_epoch = episode + best_step = step + best_epoch_training_time = ( + time.time() - start_time_epoch + ) # datetime.now() - start_time_epoch + self.global_params.actions_rewards["episode"].append(episode) + self.global_params.actions_rewards["step"].append(step) + self.global_params.actions_rewards["reward"].append(reward) + # Showing stats in screen for monitoring. Showing every 'save_every_step' value + if not step % self.env_params.save_every_step: + log.logger.info( + f"SHOWING BATCH OF STEPS\n" + f"current_max_reward = {current_max_reward}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"current epoch = {episode}\n" + f"current step = {step}\n" + f"best epoch so far = {best_epoch}\n" + f"best step so far = {best_step}\n" + f"best_epoch_training_time = {best_epoch_training_time}\n" + ) + # print_messages( + # "SHOWING BATCH OF STEPS", + # episode=episode, + # step=step, + # cumulated_reward=cumulated_reward, + # epsilon=epsilon, + # best_epoch=best_epoch, + # best_step=best_step, + # current_max_reward=current_max_reward, + # best_epoch_training_time=best_epoch_training_time, + # ) + + # Reach Finish Line!!! + if env.is_finish: + np.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_FINISHLINE_Circuit-{self.environment.environment['town']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}-qtable.npy", + qlearn.q, + ) + qlearn.save_qtable_pickle( + self.environment.environment, + self.global_params.models_dir, + qlearn, + cumulated_reward, + episode, + step, + epsilon, + ) + # qlearn.save_model( + # self.environment.environment, + # self.global_params.models_dir, + # qlearn, + # cumulated_reward, + # episode, + # step, + # epsilon, + # ) + + print_messages( + "FINISH LINE", + episode=episode, + step=step, + cumulated_reward=cumulated_reward, + epsilon=epsilon, + ) + log.logger.info( + f"\nFINISH LINE\n" + f"in episode = {episode}\n" + f"steps = {step}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"epsilon = {epsilon}\n" + ) - # env = gym.make(self.env_params.env_name, **self.environment.environment) + # End epoch + if step > self.env_params.estimated_steps: + done = True + np.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['town']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}-qtable.npy", + qlearn.q, + ) + qlearn.save_qtable_pickle( + self.environment.environment, + self.global_params.models_dir, + qlearn, + cumulated_reward, + episode, + step, + epsilon, + ) + # qlearn.save_model( + # self.environment.environment, + # self.global_params.models_dir, + # qlearn, + # cumulated_reward, + # episode, + # step, + # epsilon, + # ) + + print_messages( + "EPISODE COMPLETED", + episode=episode, + step=step, + cumulated_reward=cumulated_reward, + epsilon=epsilon, + ) + log.logger.info( + f"\nEPISODE COMPLETED\n" + f"in episode = {episode}\n" + f"steps = {step}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"epsilon = {epsilon}\n" + ) - print(f"\n enter main()\n") + # check out for Carla Server (end of every step) + ## ----------- checking for Carla Server is working + env.checking_carla_server(self.environment.environment["town"]) - # ---------------------------- - # launch client and world (sync or async) - # ---------------------------- - # client = carla.Client( - # self.environment.environment["carla_server"], - # self.environment.environment["carla_client"], - # ) - # client.set_timeout(2.0) - # world = client.get_world() - # bsc = BasicSynchronousClient(world) - # bsc.setup_car() - # bsc.setup_camera() - # bsc.set_synchronous_mode(True) + ######################################## + # collect stats in every epoch + # + ######################################## - # ------------------------------- - # visualize sensors - # ------------------------------- - # display_manager = DisplayManager( - # grid_size=[2, 3], - # window_size=[1500, 800], - # ) + ############ intrinsic + finish_time_epoch = time.time() # datetime.now() - # Then, SensorManager can be used to spawn RGBCamera, LiDARs and SemanticLiDARs as needed - # and assign each of them to a grid position, + self.global_params.im_general["episode"].append(episode) + self.global_params.im_general["step"].append(step) + self.global_params.im_general["cumulated_reward"].append(cumulated_reward) + self.global_params.im_general["epsilon"].append(epsilon) + self.global_params.im_general["epoch_time"].append( + finish_time_epoch - start_time_epoch + ) + self.global_params.im_general["lane_changed"].append( + len(env.lane_changing_hist) + ) + self.global_params.im_general["distance_to_finish"].append( + env.dist_to_finish + ) - # -- General view - """ - camera_general = SensorManager( - world, - display_manager, - "RGBCamera", - carla.Transform(carla.Location(x=-4, z=2.4), carla.Rotation(yaw=+00)), - bsc.car, - {}, - display_pos=[0, 0], - ) - # -- RGB front camera - camera_rgb_front = SensorManager( - world, - display_manager, - "RGBCamera", - carla.Transform(carla.Location(x=2, z=1), carla.Rotation(yaw=+00)), - bsc.car, - {}, - display_pos=[0, 1], - ) - camera_depth = SensorManager( - world, - display_manager, - "DepthLogarithmicCamera", - carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=+00)), - bsc.car, - {}, - display_pos=[0, 2], - ) - camera_semantic = SensorManager( - world, - display_manager, - "SemanticCamera", - carla.Transform(carla.Location(x=2, z=1), carla.Rotation(yaw=+00)), - bsc.car, - {}, - display_pos=[1, 0], - ) - """ - # ------------------------------- - # Load Environment - # ------------------------------- - # self.environment.environment["world"] = world - # self.environment.environment["bsc"] = bsc - # self.environment.environment["camera_rgb_front"] = camera_rgb_front - # self.environment.environment["display_manager"] = display_manager - env = gym.make(self.env_params.env_name, **self.environment.environment) + # print(f"{self.global_params.time_steps =}") - log = LoggingHandler(self.log_file) + ### FPS + fps_m = [values for values in self.global_params.time_steps.values()] + fps_mean = sum(fps_m) / len(self.global_params.time_steps) - start_time = datetime.now() - best_epoch = 1 - current_max_reward = 0 - best_step = 0 - best_epoch_training_time = 0 - epsilon = self.environment.environment["epsilon"] - epsilon_decay = epsilon / (self.env_params.total_episodes) - # states_counter = {} - ## --- using epsilon reduced - epsilon = epsilon / 2 - # ------------------------------- - ## --- init Qlearn - # print(f"\nstates_len = {len(self.global_params.states_set)}") - # print(f"\nactions_len = {len(self.global_params.actions_set)}") - qlearn = QLearnCarla( - len(self.global_params.states_set), - self.global_params.actions, - len(self.global_params.actions_set), - self.environment.environment["epsilon"], - self.environment.environment["alpha"], - self.environment.environment["gamma"], - self.environment.environment["num_regions"], - ) - # print(f"qlearn.q_table = {qlearn.q_table}") - # print(f"len qlearn.q_table = {len(qlearn.q_table)}") - # print(f"type qlearn.q_table = {type(qlearn.q_table)}") - # print(f"shape qlearn.q_table = {np.shape(qlearn.q_table)}") - # print(f"size qlearn.q_table = {np.size(qlearn.q_table)}") - # while True: - # world.tick() - # display_manager.render() + sorted_time_steps = OrderedDict( + sorted(self.global_params.time_steps.items(), key=lambda x: x[1]) + ) + fps_median = median(sorted_time_steps.values()) + # print(f"{fps_mean =}") + # print(f"{fps_median =}") + self.global_params.im_general["FPS_avg"].append(fps_mean) + self.global_params.im_general["FPS_median"].append(fps_median) + + stats_frame = StatsDataFrame() + stats_frame.save_dataframe_stats( + self.environment.environment, + self.global_params.metrics_graphics_dir, + self.global_params.im_general, + ) - ## ------------- START TRAINING -------------------- - for episode in tqdm( - range(1, self.env_params.total_episodes + 1), - ascii=True, - unit="episodes", - ): + """ + ### Q_tabla + for key, value in qlearn.q.items(): + self.global_params.im_q_tabla["state"].append(key[0]) + self.global_params.im_q_tabla["action"].append(key[1]) + self.global_params.im_q_tabla["q_value"].append(value) + stats_frame.save_dataframe_stats( + self.environment.environment, + self.global_params.metrics_graphics_dir, + self.global_params.im_q_tabla, + True, + ) + """ + + """ + ### States, Actions counter in every epoch + for key, value in self.global_params.states_actions_counter.items(): + self.global_params.im_state_actions_counter["epoch"].append(key[0]) + self.global_params.im_state_actions_counter["state"].append(key[1]) + self.global_params.im_state_actions_counter["action"].append(key[2]) + self.global_params.im_state_actions_counter["counter"].append(value) + stats_frame.save_dataframe_stats( + self.environment.environment, + self.global_params.metrics_graphics_dir, + self.global_params.im_state_actions_counter, + sac=True, + ) + """ - # env.display_manager.render() - observation = env.reset() - # if observation.any(): - # print(f"hay observation = {observation}\n") - # print(f"\nin main() observation = {observation}") + ## showing q_table every determined steps + # if not episode % self.env_params.save_episodes: + # print(f"\n\tQ-table {qlearn.q}") + # print(f"\n\tsac {self.global_params.states_actions_counter}") - done = False - cumulated_reward = 0 - step = 0 - start_time_epoch = datetime.now() - ## reset env() - # print(f"bsc = {bsc}") + ######################################## + # + # + ######################################## - for j in range(0, 4): - if self.environment.environment["sync"]: - env.world.tick() - else: - env.world.wait_for_tick() - # env.display_manager.render_new() - # while not done: - step += 1 - # print(f"step = {step}") - # print(f"observation = {observation}") - # Pick an action based on the current state - action = qlearn.select_action(observation) - # print(f"action = {action}") - # Execute the action and get feedback - new_observation, reward, done, _ = env.step(action) - # print( - # f"j = {j}, step = {step}, action = {action}, new_observation = {new_observation}, reward = {reward}, done = {done}, observation = {observation}" + # Save best lap + if ( + cumulated_reward - self.environment.environment["rewards"]["penal"] + ) >= current_max_reward: + self.global_params.best_current_epoch["best_epoch"].append(best_epoch) + self.global_params.best_current_epoch["highest_reward"].append( + cumulated_reward + ) + self.global_params.best_current_epoch["best_step"].append(best_step) + self.global_params.best_current_epoch[ + "best_epoch_training_time" + ].append(best_epoch_training_time) + self.global_params.best_current_epoch[ + "current_total_training_time" + ].append(datetime.now() - start_time) + save_carla_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.best_current_epoch, + ) + + np.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['town']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward - self.environment.environment['rewards']['penal'])}-qtable.npy", + qlearn.q, + ) + qlearn.save_qtable_pickle( + self.environment.environment, + self.global_params.models_dir, + qlearn, + cumulated_reward, + episode, + step, + epsilon, + ) + log.logger.info( + f"\nsaving best lap\n" + f"in episode = {episode}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"current_max_reward = {current_max_reward}\n" + f"steps = {step}\n" + f"epsilon = {epsilon}\n" + ) + # print_messages( + # "saving best lap", + # episode=episode, + # cumulated_reward=cumulated_reward, + # current_max_reward=current_max_reward, + # step=step, + # epsilon=epsilon, # ) - cumulated_reward += reward - pygame.display.flip() - # qlearn.learn(observation, action, reward, new_observation) - # observation = new_observation - # print("llego aqui") - ## ------------ destroy actors - # .display_manager.destroy() - # print(env) - env.destroy_all_actors() - # for actor in env.actor_list[::-1]: - # print(f"\nin main() actor : {actor}\n") - # actor.destroy() - # print(f"no llego aqui\n") + ### Q_tabla + self.global_params.im_q_table = { + "state": [], + "action": [], + "q_value": [], + } + for key, value in qlearn.q.items(): + self.global_params.im_q_table["state"].append(key[0]) + self.global_params.im_q_table["action"].append(key[1]) + self.global_params.im_q_table["q_value"].append(value) + stats_frame.save_dataframe_stats( + self.environment.environment, + self.global_params.metrics_graphics_dir, + self.global_params.im_q_table, + True, + ) + # end of training by: + # training time setting: 2 hours, 15 hours... + # num epochs - # env.actor_list = [] - # bsc.destroy_all_actors() + if ( + datetime.now() - timedelta(hours=self.global_params.training_time) + > start_time + ) or (episode > self.env_params.total_episodes): + if ( + cumulated_reward - self.environment.environment["rewards"]["penal"] + ) >= current_max_reward: + np.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['town']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}-qtable.npy", + qlearn.q, + ) + log.logger.info( + f"\nTraining Time over\n" + f"current_max_reward = {current_max_reward}\n" + f"epoch = {episode}\n" + f"step = {step}\n" + f"epsilon = {epsilon}\n" + ) + # print_messages( + # "Training Time over", + # episode=episode, + # cumulated_reward=cumulated_reward, + # current_max_reward=current_max_reward, + # step=step, + # epsilon=epsilon, + # ) + break + # save best values every save_episode times + self.global_params.ep_rewards.append(cumulated_reward) + if not episode % self.env_params.save_episodes: + self.global_params.aggr_ep_rewards = save_batch( + episode, + step, + start_time_epoch, + start_time, + self.global_params, + self.env_params, + ) + save_carla_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.aggr_ep_rewards, + ) + log.logger.info( + f"\nsaving BATCH\n" + f"current_max_reward = {current_max_reward}\n" + f"best_epoch = {best_epoch}\n" + f"best_step = {best_step}\n" + f"best_epoch_training_time = {best_epoch_training_time}\n" + ) + # print_messages( + # "saving BATCH", + # best_epoch=best_epoch, + # best_epoch_training_time=best_epoch_training_time, + # current_max_reward=current_max_reward, + # best_step=best_step, + # ) + + # updating epsilon for exploration + if epsilon > self.environment.environment["epsilon_min"]: + # self.epsilon *= self.epsilon_discount + epsilon -= epsilon_decay + epsilon = qlearn.updateEpsilon( + max(self.environment.environment["epsilon_min"], epsilon) + ) + + ## ------------ destroy actors + env.destroy_all_actors() # ----------- end for - # if env.display_manager: - # env.display_manager.destroy() - # bsc.set_synchronous_mode(False) - # bsc.destroy_all_actors() - # bsc.camera.destroy() - # bsc.car.destroy() - # pygame.quit() + ### States, Actions counter in every epoch + for key, value in self.global_params.states_actions_counter.items(): + self.global_params.im_state_actions_counter["epoch"].append(key[0]) + self.global_params.im_state_actions_counter["state"].append(key[1]) + self.global_params.im_state_actions_counter["action"].append(key[2]) + self.global_params.im_state_actions_counter["counter"].append(value) + stats_frame.save_dataframe_stats( + self.environment.environment, + self.global_params.metrics_graphics_dir, + self.global_params.im_state_actions_counter, + sac=True, + ) + env.close() - #########################################################################33 - #########################################################################33 - #########################################################################33 - #########################################################################33 - #########################################################################33 - #########################################################################33 - #########################################################################33 - ################## - ################## - ################## - ################## - ################## - def ma__in(self): + #################################################################################### + #################################################################################### + #################################################################################### + #################################################################################### + + def AKAKAKAAKKAKAAKmain(self): """ - Funciona para Manual Control: crea World, HUD y sensores. Lanza Pygame pero solo 1 ventana, y no se ven bien los sensores - pero funciona...dejamos ahi para control + QlearnCarla dictionnary """ + log = LoggingHandler(self.log_file) env = gym.make(self.env_params.env_name, **self.environment.environment) - # TODO: Pygame - pygame.init() - pygame.font.init() - world = None - - # ---------------------------- - # launch client and world (sync or async) - # ---------------------------- - client = carla.Client( - self.environment.environment["carla_server"], - self.environment.environment["carla_client"], + start_time = datetime.now() + best_epoch = 1 + current_max_reward = 0 + best_step = 0 + best_epoch_training_time = 0 + epsilon = self.environment.environment["epsilon"] + epsilon_decay = epsilon / (self.env_params.total_episodes) + # epsilon = epsilon / 2 + # ------------------------------- + # log.logger.info( + # f"\nactions_len = {len(self.global_params.actions_set)}\n" + # f"actions_range = {range(len(self.global_params.actions_set))}\n" + # f"actions = {self.global_params.actions_set}\n" + # f"epsilon = {epsilon}\n" + # f"epsilon_decay = {epsilon_decay}\n" + # f"alpha = {self.environment.environment['alpha']}\n" + # f"gamma = {self.environment.environment['gamma']}\n" + # ) + print_messages( + "main()", + actions_len=len(self.global_params.actions_set), + actions_range=range(len(self.global_params.actions_set)), + actions=self.global_params.actions_set, + epsilon=epsilon, + epsilon_decay=epsilon_decay, + alpha=self.environment.environment["alpha"], + gamma=self.environment.environment["gamma"], ) - client.set_timeout(2.0) - sim_world = client.get_world() + ## --- init Qlearn + qlearn = QLearnCarla( + actions=range(len(self.global_params.actions_set)), + epsilon=self.environment.environment["epsilon"], + alpha=self.environment.environment["alpha"], + gamma=self.environment.environment["gamma"], + ) + # log.logger.info( + # f"\nqlearn.q_table = {qlearn.q_table}", + # ) + # print_messages( + # "", + # qlearn_q_table=qlearn.q_table, + # ) + ## ------------- START TRAINING -------------------- + for episode in tqdm( + range(1, self.env_params.total_episodes + 1), + ascii=True, + unit="episodes", + ): + time.sleep(0.1) + done = False + cumulated_reward = 0 + step = 0 + start_time_epoch = datetime.now() - if self.environment.environment["sync"]: - original_settings = sim_world.get_settings() - settings = sim_world.get_settings() - if not settings.synchronous_mode: - settings.synchronous_mode = True - settings.fixed_delta_seconds = 0.05 - sim_world.apply_settings(settings) + observation = env.reset() + state = "-".join(map(str, observation)) + print_messages( + "in episode", + episode=episode, + observation=observation, + state=state, + # type_observation=type(observation), + # type_state=type(state), + ) - # TODO: understand .get_trafficmanager() - # traffic_manager = client.get_trafficmanager() - # traffic_manager.set_synchronous_mode(True) + while not done: + # if self.environment.environment["sync"]: + env.world.tick() + # else: + # env.world.wait_for_tick() + step += 1 + action = qlearn.select_action(state) + # print(f"{action = }") + new_observation, reward, done, _ = env.step(action) + # time.sleep(4) - # ---------------------------- - # Weather: Static - # Traffic and pedestrians: No - # ---------------------------- - weather = self.environment.environment["weather"] - traffic = self.environment.environment["traffic_pedestrians"] - if weather != "dynamic" and traffic is False: - pass + cumulated_reward += reward + next_state = "-".join(map(str, new_observation)) + """ + print_messages( + "", + episode=episode, + step=step, + action=action, + state=state, + new_observation=new_observation, + reward=reward, + done=done, + next_state=next_state, + current_max_reward=current_max_reward, + ) + """ + """ + log.logger.info( + f"\n step = {step}\n" + f"action = {action}\n" + f"actions type = {type(action)}\n" + f"state = {state}\n" + # f"observation[0]= {observation[0]}\n" + # f"observation type = {type(observation)}\n" + # f"observation[0] type = {type(observation[0])}\n" + f"new_observation = {new_observation}\n" + f"type new_observation = {type(new_observation)}\n" + f"reward = {reward}\n" + f"done = {done}\n" + f"next_state = {next_state}\n" + f"type new_observation = {type(new_observation)}\n" + f"current_max_reward = {current_max_reward}\n" + ) + """ - # ------------------------------- - # Pygame, HUD, world - # ------------------------------- - display = pygame.display.set_mode( - ( - self.environment.environment["width_image"], - self.environment.environment["height_image"], - ), - pygame.HWSURFACE | pygame.DOUBLEBUF, - ) - display.fill((0, 0, 0)) - pygame.display.flip() + qlearn.learn(state, action, reward, next_state) + state = next_state - hud = HUD( - self.environment.environment["width_image"], - self.environment.environment["height_image"], - ) - world = World(sim_world, hud, self.environment.environment) - # controller = KeyboardControl(world, args.autopilot) + ## showing q_table every determined steps + if not episode % self.env_params.save_episodes: + print(f"\n\tQ-table {qlearn.q_table}") - if self.environment.environment["sync"]: - sim_world.tick() - else: - sim_world.wait_for_tick() + # print_messages( + # "", + # next_state=next_state, + # state=state, + # ) + # env.display_manager.render() + # render params - clock = pygame.time.Clock() + render_params_left_bottom( + episode=episode, + step=step, + action=action, + epsilon=epsilon, + new_observation=new_observation, + reward_in_step=reward, + cumulated_reward=cumulated_reward, + _="------------------------", + current_max_reward=current_max_reward, + best_epoch=best_epoch, + best_step=best_step, + done=done, + ) - # TODO: env.recorder_file() to save trainings + # best episode and step's stats + if current_max_reward <= cumulated_reward: + current_max_reward = cumulated_reward + best_epoch = episode + best_step = step + best_epoch_training_time = datetime.now() - start_time_epoch + self.global_params.actions_rewards["episode"].append(episode) + self.global_params.actions_rewards["step"].append(step) + self.global_params.actions_rewards["reward"].append(reward) + # Showing stats in screen for monitoring. Showing every 'save_every_step' value + if not step % self.env_params.save_every_step: + log.logger.info( + f"SHOWING BATCH OF STEPS\n" + f"current_max_reward = {current_max_reward}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"current epoch = {episode}\n" + f"current step = {step}\n" + f"best epoch so far = {best_epoch}\n" + f"best step so far = {best_step}\n" + f"best_epoch_training_time = {best_epoch_training_time}\n" + ) + # print_messages( + # "SHOWING BATCH OF STEPS", + # episode=episode, + # step=step, + # cumulated_reward=cumulated_reward, + # epsilon=epsilon, + # best_epoch=best_epoch, + # best_step=best_step, + # current_max_reward=current_max_reward, + # best_epoch_training_time=best_epoch_training_time, + # ) + # End epoch + if step > self.env_params.estimated_steps: + done = True + np.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['town']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}-qtable.npy", + qlearn.q_table, + ) + print_messages( + "EPISODE COMPLETED", + episode=episode, + step=step, + cumulated_reward=cumulated_reward, + epsilon=epsilon, + ) + log.logger.info( + f"\nEPISODE COMPLETED\n" + f"in episode = {episode}\n" + f"steps = {step}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"epsilon = {epsilon}\n" + ) - log = LoggingHandler(self.log_file) + # check out for Carla Server (end of every step) + ## ----------- checking for Carla Server is working + env.checking_carla_server() - # ------------------------------- - # visualize sensors - # ------------------------------- - # display_manager = DisplayManager( - # grid_size=[2, 3], - # window_size=[ - # self.environment.environment["width_image"], - # self.environment.environment["height_image"], - # ], - # ) + # Save best lap + if ( + cumulated_reward - self.environment.environment["rewards"]["penal"] + ) >= current_max_reward: + self.global_params.best_current_epoch["best_epoch"].append(best_epoch) + self.global_params.best_current_epoch["highest_reward"].append( + cumulated_reward + ) + self.global_params.best_current_epoch["best_step"].append(best_step) + self.global_params.best_current_epoch[ + "best_epoch_training_time" + ].append(best_epoch_training_time) + self.global_params.best_current_epoch[ + "current_total_training_time" + ].append(datetime.now() - start_time) + save_carla_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.best_current_epoch, + ) - # Then, SensorManager can be used to spawn RGBCamera, LiDARs and SemanticLiDARs as needed - # and assign each of them to a grid position, - SensorManager( - sim_world, - display, - "RGBCamera", - carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=-90)), - world.player, - {}, - display_pos=[0, 0], - ) - SensorManager( - sim_world, - display, - "RGBCamera", - carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=+00)), - world.player, - {}, - display_pos=[0, 1], - ) - SensorManager( - sim_world, - display, - "RGBCamera", - carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=+90)), - world.player, - {}, - display_pos=[0, 2], - ) - SensorManager( - sim_world, - display, - "RGBCamera", - carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=180)), - world.player, - {}, - display_pos=[1, 1], - ) + np.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['town']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward - self.environment.environment['rewards']['penal'])}-qtable.npy", + qlearn.q_table, + ) + log.logger.info( + f"\nsaving best lap\n" + f"in episode = {episode}\n" + f"cumulated_reward = {cumulated_reward}\n" + f"current_max_reward = {current_max_reward}\n" + f"steps = {step}\n" + f"epsilon = {epsilon}\n" + ) + # print_messages( + # "saving best lap", + # episode=episode, + # cumulated_reward=cumulated_reward, + # current_max_reward=current_max_reward, + # step=step, + # epsilon=epsilon, + # ) - SensorManager( - sim_world, - display, - "LiDAR", - carla.Transform(carla.Location(x=0, z=2.4)), - world.player, - { - "channels": "64", - "range": "100", - "points_per_second": "250000", - "rotation_frequency": "20", - }, - display_pos=[1, 0], - ) - SensorManager( - sim_world, - display, - "SemanticLiDAR", - carla.Transform(carla.Location(x=0, z=2.4)), - world.player, - { - "channels": "64", - "range": "100", - "points_per_second": "100000", - "rotation_frequency": "20", - }, - display_pos=[1, 2], - ) + # end of training by: + # training time setting: 2 hours, 15 hours... + # num epochs - # ------------------------------- - # Load Environment - # ------------------------------- + if ( + datetime.now() - timedelta(hours=self.global_params.training_time) + > start_time + ) or (episode > self.env_params.total_episodes): + if ( + cumulated_reward - self.environment.environment["rewards"]["penal"] + ) >= current_max_reward: + np.save( + f"{self.global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{self.environment.environment['town']}_States-{self.environment.environment['states']}_Actions-{self.environment.environment['action_space']}_Rewards-{self.environment.environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}-qtable.npy", + qlearn.q_table, + ) + log.logger.info( + f"\nTraining Time over\n" + f"current_max_reward = {current_max_reward}\n" + f"epoch = {episode}\n" + f"step = {step}\n" + f"epsilon = {epsilon}\n" + ) + # print_messages( + # "Training Time over", + # episode=episode, + # cumulated_reward=cumulated_reward, + # current_max_reward=current_max_reward, + # step=step, + # epsilon=epsilon, + # ) + break + + # save best values every save_episode times + self.global_params.ep_rewards.append(cumulated_reward) + if not episode % self.env_params.save_episodes: + self.global_params.aggr_ep_rewards = save_batch( + episode, + step, + start_time_epoch, + start_time, + self.global_params, + self.env_params, + ) + save_carla_dataframe_episodes( + self.environment.environment, + self.global_params.metrics_data_dir, + self.global_params.aggr_ep_rewards, + ) + log.logger.info( + f"\nsaving BATCH\n" + f"current_max_reward = {current_max_reward}\n" + f"best_epoch = {best_epoch}\n" + f"best_step = {best_step}\n" + f"best_epoch_training_time = {best_epoch_training_time}\n" + ) + # print_messages( + # "saving BATCH", + # best_epoch=best_epoch, + # best_epoch_training_time=best_epoch_training_time, + # current_max_reward=current_max_reward, + # best_step=best_step, + # ) + # updating epsilon for exploration + if epsilon > self.environment.environment["epsilon_min"]: + # self.epsilon *= self.epsilon_discount + epsilon -= epsilon_decay + epsilon = qlearn.update_epsilon( + max(self.environment.environment["epsilon_min"], epsilon) + ) + + ## ------------ destroy actors + env.destroy_all_actors() + # env.display_manager.destroy() + # ----------- end for + + # env.close() + + def main_____(self): + """ + old Qlearn Dictionnary + """ + + log = LoggingHandler(self.log_file) + + ## Load Environment + env = gym.make(self.env_params.env_name, **self.environment.environment) start_time = datetime.now() best_epoch = 1 @@ -555,256 +962,48 @@ def ma__in(self): best_step = 0 best_epoch_training_time = 0 epsilon = self.environment.environment["epsilon"] - epsilon_decay = epsilon / (self.env_params.total_episodes) + epsilon_decay = epsilon / (self.env_params.total_episodes // 2) # states_counter = {} - ## --- using epsilon reduced - epsilon = epsilon / 2 - # ------------------------------- + + log.logger.info( + f"\nactions_len = {len(self.global_params.actions_set)}\n" + f"actions_range = {range(len(self.global_params.actions_set))}\n" + f"actions = {self.global_params.actions_set}\n" + f"epsilon = {epsilon}\n" + f"epsilon_decay = {epsilon_decay}\n" + f"alpha = {self.environment.environment['alpha']}\n" + f"gamma = {self.environment.environment['gamma']}\n" + ) ## --- init Qlearn - qlearn = QLearnCarla( - len(self.global_params.states_set), - self.global_params.actions, - len(self.global_params.actions_set), - self.environment.environment["epsilon"], - self.environment.environment["alpha"], - self.environment.environment["gamma"], - self.environment.environment["num_regions"], + qlearn = QLearn( + actions=range(len(self.global_params.actions_set)), + epsilon=self.environment.environment["epsilon"], + alpha=self.environment.environment["alpha"], + gamma=self.environment.environment["gamma"], ) + log.logger.info(f"\nqlearn.q = {qlearn.q}") - while True: - if self.environment.environment["sync"]: - sim_world.tick() - clock.tick_busy_loop(60) - # if controller.parse_events(client, world, clock, args.sync): - # return + ## retraining q model + if self.environment.environment["mode"] == "retraining": + qlearn.q = qlearn.load_pickle_model( + f"{self.global_params.models_dir}/{self.environment.environment['retrain_qlearn_model_name']}" + ) + log.logger.info(f"\nqlearn.q = {qlearn.q}") - # display_manager.render() - world.tick(clock) - world.render(display) - pygame.display.flip() + ## ------------- START TRAINING -------------------- + for episode in tqdm( + range(1, self.env_params.total_episodes + 1), + ascii=True, + unit="episodes", + ): + done = False + cumulated_reward = 0 + step = 0 + start_time_epoch = datetime.now() - # TODO: - if display_manager: - display_manager.destroy() - if world is not None: - world.destroy() - - pygame.quit() - env.close() - - #########################################################################33 - #########################################################################33 - #########################################################################33 - #########################################################################33 - #########################################################################33 - #########################################################################33 - #########################################################################33 - ################## - ################## - ################## - ################## - ################## - - def __main(self): - """ - Implementamos synchronous_mode.py que es mas sencillo - """ - env = gym.make(self.env_params.env_name, **self.environment.environment) - - # ---------------------------- - # pygame, display - # ---------------------------- - pygame.init() - display = pygame.display.set_mode( - (800, 600), pygame.HWSURFACE | pygame.DOUBLEBUF - ) - font = get_font() - clock = pygame.time.Clock() - - # ---------------------------- - # launch client and world - # ---------------------------- - client = carla.Client( - self.environment.environment["carla_server"], - self.environment.environment["carla_client"], - ) - client.set_timeout(2.0) - world = client.get_world() - - # ---------------------------- - # blueprint - # ---------------------------- - m = world.get_map() - start_pose = random.choice(m.get_spawn_points()) - waypoint = m.get_waypoint(start_pose.location) - - blueprint_library = world.get_blueprint_library() - - # ---------------------------- - # car - # ---------------------------- - actor_list = [] - - vehicle = world.spawn_actor( - random.choice(blueprint_library.filter("vehicle.*")), start_pose - ) - actor_list.append(vehicle) - vehicle.set_simulate_physics(False) - # ---------------------------- - # RGB, Semantic - # ---------------------------- - camera_rgb = world.spawn_actor( - blueprint_library.find("sensor.camera.rgb"), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle, - ) - actor_list.append(camera_rgb) - - camera_semseg = world.spawn_actor( - blueprint_library.find("sensor.camera.semantic_segmentation"), - carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), - attach_to=vehicle, - ) - actor_list.append(camera_semseg) - - log = LoggingHandler(self.log_file) - - # ------------------------------- - # Load Environment - # ------------------------------- - - start_time = datetime.now() - best_epoch = 1 - current_max_reward = 0 - best_step = 0 - best_epoch_training_time = 0 - epsilon = self.environment.environment["epsilon"] - epsilon_decay = epsilon / (self.env_params.total_episodes) - # states_counter = {} - - # ------------------------------- - ## --- init Qlearn - qlearn = QLearnCarla( - len(self.global_params.states_set), - self.global_params.actions, - len(self.global_params.actions_set), - self.environment.environment["epsilon"], - self.environment.environment["alpha"], - self.environment.environment["gamma"], - self.environment.environment["num_regions"], - ) - - ## --- using epsilon reduced - epsilon = epsilon / 2 - - try: - # Create a synchronous mode context. - with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=30) as sync_mode: - while True: - if should_quit(): - return - clock.tick() - - # Advance the simulation and wait for the data. - snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=2.0) - - # Choose the next waypoint and update the car location. - waypoint = random.choice(waypoint.next(1.5)) - vehicle.set_transform(waypoint.transform) - - image_semseg.convert(carla.ColorConverter.CityScapesPalette) - fps = round(1.0 / snapshot.timestamp.delta_seconds) - - # Draw the display. - draw_image(display, image_rgb) - draw_image(display, image_semseg, blend=True) - display.blit( - font.render( - "% 5d FPS (real)" % clock.get_fps(), True, (255, 255, 255) - ), - (8, 10), - ) - display.blit( - font.render( - "% 5d FPS (simulated)" % fps, True, (255, 255, 255) - ), - (8, 28), - ) - pygame.display.flip() - finally: - for actor in actor_list: - actor.destroy() - - pygame.quit() - env.close() - - ########################################################################################### - ########################################################################################### - ########################################################################################### - ########################################################################################### - ########################################################################################### - ########################################################################################### - ########################################################################################### - ########################################################################################### - - def main_____(self): - """ - Qlearn Dictionnary - """ - - log = LoggingHandler(self.log_file) - - ## Load Environment - env = gym.make(self.env_params.env_name, **self.environment.environment) - - start_time = datetime.now() - best_epoch = 1 - current_max_reward = 0 - best_step = 0 - best_epoch_training_time = 0 - epsilon = self.environment.environment["epsilon"] - epsilon_decay = epsilon / (self.env_params.total_episodes // 2) - # states_counter = {} - - log.logger.info( - f"\nactions_len = {len(self.global_params.actions_set)}\n" - f"actions_range = {range(len(self.global_params.actions_set))}\n" - f"actions = {self.global_params.actions_set}\n" - f"epsilon = {epsilon}\n" - f"epsilon_decay = {epsilon_decay}\n" - f"alpha = {self.environment.environment['alpha']}\n" - f"gamma = {self.environment.environment['gamma']}\n" - ) - ## --- init Qlearn - qlearn = QLearn( - actions=range(len(self.global_params.actions_set)), - epsilon=self.environment.environment["epsilon"], - alpha=self.environment.environment["alpha"], - gamma=self.environment.environment["gamma"], - ) - log.logger.info(f"\nqlearn.q = {qlearn.q}") - - ## retraining q model - if self.environment.environment["mode"] == "retraining": - qlearn.q = qlearn.load_pickle_model( - f"{self.global_params.models_dir}/{self.environment.environment['retrain_qlearn_model_name']}" - ) - log.logger.info(f"\nqlearn.q = {qlearn.q}") - - ## ------------- START TRAINING -------------------- - for episode in tqdm( - range(1, self.env_params.total_episodes + 1), - ascii=True, - unit="episodes", - ): - done = False - cumulated_reward = 0 - step = 0 - start_time_epoch = datetime.now() - - ## reset env() - observation = env.reset() - state = "".join(map(str, observation)) + ## reset env() + observation = env.reset() + state = "".join(map(str, observation)) print(f"observation: {observation}") print(f"observation type: {type(observation)}") @@ -1010,3 +1209,552 @@ def main_____(self): ) env.close() + + ########################################################################################### + ########################################################################################### + ########################################################################################### + ########################################################################################### + ########################################################################################### + ########################################################################################### + ########################################################################################### + ########################################################################################### + ###################################################################################################### + def AAAAmain(self): + """ + esta funcionando, muestra la imagen en Pygame pero no se actualiza en el step + """ + # print(f"\nself.env_params.env_name = {self.env_params.env_name}\n") + + # env = gym.make(self.env_params.env_name, **self.environment.environment) + + print(f"\n enter main()\n") + + # ---------------------------- + # launch client and world (sync or async) + # ---------------------------- + # client = carla.Client( + # self.environment.environment["carla_server"], + # self.environment.environment["carla_client"], + # ) + # client.set_timeout(2.0) + # world = client.get_world() + # bsc = BasicSynchronousClient(world) + # bsc.setup_car() + # bsc.setup_camera() + # bsc.set_synchronous_mode(True) + + # ------------------------------- + # visualize sensors + # ------------------------------- + # display_manager = DisplayManager( + # grid_size=[2, 3], + # window_size=[1500, 800], + # ) + + # Then, SensorManager can be used to spawn RGBCamera, LiDARs and SemanticLiDARs as needed + # and assign each of them to a grid position, + + # -- General view + """ + camera_general = SensorManager( + world, + display_manager, + "RGBCamera", + carla.Transform(carla.Location(x=-4, z=2.4), carla.Rotation(yaw=+00)), + bsc.car, + {}, + display_pos=[0, 0], + ) + # -- RGB front camera + camera_rgb_front = SensorManager( + world, + display_manager, + "RGBCamera", + carla.Transform(carla.Location(x=2, z=1), carla.Rotation(yaw=+00)), + bsc.car, + {}, + display_pos=[0, 1], + ) + camera_depth = SensorManager( + world, + display_manager, + "DepthLogarithmicCamera", + carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=+00)), + bsc.car, + {}, + display_pos=[0, 2], + ) + camera_semantic = SensorManager( + world, + display_manager, + "SemanticCamera", + carla.Transform(carla.Location(x=2, z=1), carla.Rotation(yaw=+00)), + bsc.car, + {}, + display_pos=[1, 0], + ) + """ + # ------------------------------- + # Load Environment + # ------------------------------- + # self.environment.environment["world"] = world + # self.environment.environment["bsc"] = bsc + # self.environment.environment["camera_rgb_front"] = camera_rgb_front + # self.environment.environment["display_manager"] = display_manager + env = gym.make(self.env_params.env_name, **self.environment.environment) + + log = LoggingHandler(self.log_file) + + start_time = datetime.now() + best_epoch = 1 + current_max_reward = 0 + best_step = 0 + best_epoch_training_time = 0 + epsilon = self.environment.environment["epsilon"] + epsilon_decay = epsilon / (self.env_params.total_episodes) + # states_counter = {} + ## --- using epsilon reduced + epsilon = epsilon / 2 + # ------------------------------- + ## --- init Qlearn + # print(f"\nstates_len = {len(self.global_params.states_set)}") + # print(f"\nactions_len = {len(self.global_params.actions_set)}") + qlearn = QLearnCarla( + len(self.global_params.states_set), + self.global_params.actions, + len(self.global_params.actions_set), + self.environment.environment["epsilon"], + self.environment.environment["alpha"], + self.environment.environment["gamma"], + self.environment.environment["num_regions"], + ) + # print(f"qlearn.q_table = {qlearn.q_table}") + # print(f"len qlearn.q_table = {len(qlearn.q_table)}") + # print(f"type qlearn.q_table = {type(qlearn.q_table)}") + # print(f"shape qlearn.q_table = {np.shape(qlearn.q_table)}") + # print(f"size qlearn.q_table = {np.size(qlearn.q_table)}") + # while True: + # world.tick() + # display_manager.render() + + ## ------------- START TRAINING -------------------- + for episode in tqdm( + range(1, self.env_params.total_episodes + 1), + ascii=True, + unit="episodes", + ): + # env.display_manager.render() + observation = env.reset() + # if observation.any(): + # print(f"hay observation = {observation}\n") + # print(f"\nin main() observation = {observation}") + + done = False + cumulated_reward = 0 + step = 0 + start_time_epoch = datetime.now() + ## reset env() + # print(f"bsc = {bsc}") + + for j in range(0, 4): + if self.environment.environment["sync"]: + env.world.tick() + else: + env.world.wait_for_tick() + # env.display_manager.render_new() + # while not done: + step += 1 + # print(f"step = {step}") + # print(f"observation = {observation}") + # Pick an action based on the current state + action = qlearn.select_action(observation) + # print(f"action = {action}") + # Execute the action and get feedback + new_observation, reward, done, _ = env.step(action) + # print( + # f"j = {j}, step = {step}, action = {action}, new_observation = {new_observation}, reward = {reward}, done = {done}, observation = {observation}" + # ) + cumulated_reward += reward + pygame.display.flip() + # qlearn.learn(observation, action, reward, new_observation) + # observation = new_observation + # print("llego aqui") + ## ------------ destroy actors + # .display_manager.destroy() + # print(env) + env.destroy_all_actors() + # for actor in env.actor_list[::-1]: + # print(f"\nin main() actor : {actor}\n") + # actor.destroy() + # print(f"no llego aqui\n") + + # env.actor_list = [] + # bsc.destroy_all_actors() + + # ----------- end for + # if env.display_manager: + # env.display_manager.destroy() + # bsc.set_synchronous_mode(False) + # bsc.destroy_all_actors() + # bsc.camera.destroy() + # bsc.car.destroy() + # pygame.quit() + env.close() + + #########################################################################33 + #########################################################################33 + #########################################################################33 + #########################################################################33 + #########################################################################33 + #########################################################################33 + #########################################################################33 + ################## + ################## + ################## + ################## + ################## + def ma__in(self): + """ + Funciona para Manual Control: crea World, HUD y sensores. Lanza Pygame pero solo 1 ventana, y no se ven bien los sensores + pero funciona...dejamos ahi para control + """ + env = gym.make(self.env_params.env_name, **self.environment.environment) + + # TODO: Pygame + pygame.init() + pygame.font.init() + world = None + + # ---------------------------- + # launch client and world (sync or async) + # ---------------------------- + client = carla.Client( + self.environment.environment["carla_server"], + self.environment.environment["carla_client"], + ) + client.set_timeout(2.0) + sim_world = client.get_world() + + if self.environment.environment["sync"]: + original_settings = sim_world.get_settings() + settings = sim_world.get_settings() + if not settings.synchronous_mode: + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + sim_world.apply_settings(settings) + + # TODO: understand .get_trafficmanager() + # traffic_manager = client.get_trafficmanager() + # traffic_manager.set_synchronous_mode(True) + + # ---------------------------- + # Weather: Static + # Traffic and pedestrians: No + # ---------------------------- + weather = self.environment.environment["weather"] + traffic = self.environment.environment["traffic_pedestrians"] + if weather != "dynamic" and traffic is False: + pass + + # ------------------------------- + # Pygame, HUD, world + # ------------------------------- + display = pygame.display.set_mode( + ( + self.environment.environment["width_image"], + self.environment.environment["height_image"], + ), + pygame.HWSURFACE | pygame.DOUBLEBUF, + ) + display.fill((0, 0, 0)) + pygame.display.flip() + + hud = HUD( + self.environment.environment["width_image"], + self.environment.environment["height_image"], + ) + world = World(sim_world, hud, self.environment.environment) + # controller = KeyboardControl(world, args.autopilot) + + if self.environment.environment["sync"]: + sim_world.tick() + else: + sim_world.wait_for_tick() + + clock = pygame.time.Clock() + + # TODO: env.recorder_file() to save trainings + + log = LoggingHandler(self.log_file) + + # ------------------------------- + # visualize sensors + # ------------------------------- + # display_manager = DisplayManager( + # grid_size=[2, 3], + # window_size=[ + # self.environment.environment["width_image"], + # self.environment.environment["height_image"], + # ], + # ) + + # Then, SensorManager can be used to spawn RGBCamera, LiDARs and SemanticLiDARs as needed + # and assign each of them to a grid position, + SensorManager( + sim_world, + display, + "RGBCamera", + carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=-90)), + world.player, + {}, + display_pos=[0, 0], + ) + SensorManager( + sim_world, + display, + "RGBCamera", + carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=+00)), + world.player, + {}, + display_pos=[0, 1], + ) + SensorManager( + sim_world, + display, + "RGBCamera", + carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=+90)), + world.player, + {}, + display_pos=[0, 2], + ) + SensorManager( + sim_world, + display, + "RGBCamera", + carla.Transform(carla.Location(x=0, z=2.4), carla.Rotation(yaw=180)), + world.player, + {}, + display_pos=[1, 1], + ) + + SensorManager( + sim_world, + display, + "LiDAR", + carla.Transform(carla.Location(x=0, z=2.4)), + world.player, + { + "channels": "64", + "range": "100", + "points_per_second": "250000", + "rotation_frequency": "20", + }, + display_pos=[1, 0], + ) + SensorManager( + sim_world, + display, + "SemanticLiDAR", + carla.Transform(carla.Location(x=0, z=2.4)), + world.player, + { + "channels": "64", + "range": "100", + "points_per_second": "100000", + "rotation_frequency": "20", + }, + display_pos=[1, 2], + ) + + # ------------------------------- + # Load Environment + # ------------------------------- + + start_time = datetime.now() + best_epoch = 1 + current_max_reward = 0 + best_step = 0 + best_epoch_training_time = 0 + epsilon = self.environment.environment["epsilon"] + epsilon_decay = epsilon / (self.env_params.total_episodes) + # states_counter = {} + ## --- using epsilon reduced + epsilon = epsilon / 2 + # ------------------------------- + ## --- init Qlearn + qlearn = QLearnCarla( + len(self.global_params.states_set), + self.global_params.actions, + len(self.global_params.actions_set), + self.environment.environment["epsilon"], + self.environment.environment["alpha"], + self.environment.environment["gamma"], + self.environment.environment["num_regions"], + ) + + while True: + if self.environment.environment["sync"]: + sim_world.tick() + clock.tick_busy_loop(60) + # if controller.parse_events(client, world, clock, args.sync): + # return + + # display_manager.render() + world.tick(clock) + world.render(display) + pygame.display.flip() + + # TODO: + if display_manager: + display_manager.destroy() + if world is not None: + world.destroy() + + pygame.quit() + env.close() + + #########################################################################33 + #########################################################################33 + #########################################################################33 + #########################################################################33 + #########################################################################33 + #########################################################################33 + #########################################################################33 + ################## + ################## + ################## + ################## + ################## + + def __main(self): + """ + Implementamos synchronous_mode.py que es mas sencillo + """ + env = gym.make(self.env_params.env_name, **self.environment.environment) + + # ---------------------------- + # pygame, display + # ---------------------------- + pygame.init() + display = pygame.display.set_mode( + (800, 600), pygame.HWSURFACE | pygame.DOUBLEBUF + ) + font = get_font() + clock = pygame.time.Clock() + + # ---------------------------- + # launch client and world + # ---------------------------- + client = carla.Client( + self.environment.environment["carla_server"], + self.environment.environment["carla_client"], + ) + client.set_timeout(2.0) + world = client.get_world() + + # ---------------------------- + # blueprint + # ---------------------------- + m = world.get_map() + start_pose = random.choice(m.get_spawn_points()) + waypoint = m.get_waypoint(start_pose.location) + + blueprint_library = world.get_blueprint_library() + + # ---------------------------- + # car + # ---------------------------- + actor_list = [] + + vehicle = world.spawn_actor( + random.choice(blueprint_library.filter("vehicle.*")), start_pose + ) + actor_list.append(vehicle) + vehicle.set_simulate_physics(False) + # ---------------------------- + # RGB, Semantic + # ---------------------------- + camera_rgb = world.spawn_actor( + blueprint_library.find("sensor.camera.rgb"), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle, + ) + actor_list.append(camera_rgb) + + camera_semseg = world.spawn_actor( + blueprint_library.find("sensor.camera.semantic_segmentation"), + carla.Transform(carla.Location(x=-5.5, z=2.8), carla.Rotation(pitch=-15)), + attach_to=vehicle, + ) + actor_list.append(camera_semseg) + + log = LoggingHandler(self.log_file) + + # ------------------------------- + # Load Environment + # ------------------------------- + + start_time = datetime.now() + best_epoch = 1 + current_max_reward = 0 + best_step = 0 + best_epoch_training_time = 0 + epsilon = self.environment.environment["epsilon"] + epsilon_decay = epsilon / (self.env_params.total_episodes) + # states_counter = {} + + # ------------------------------- + ## --- init Qlearn + qlearn = QLearnCarla( + len(self.global_params.states_set), + self.global_params.actions, + len(self.global_params.actions_set), + self.environment.environment["epsilon"], + self.environment.environment["alpha"], + self.environment.environment["gamma"], + self.environment.environment["num_regions"], + ) + + ## --- using epsilon reduced + epsilon = epsilon / 2 + + try: + # Create a synchronous mode context. + with CarlaSyncMode(world, camera_rgb, camera_semseg, fps=30) as sync_mode: + while True: + if should_quit(): + return + clock.tick() + + # Advance the simulation and wait for the data. + snapshot, image_rgb, image_semseg = sync_mode.tick(timeout=2.0) + + # Choose the next waypoint and update the car location. + waypoint = random.choice(waypoint.next(1.5)) + vehicle.set_transform(waypoint.transform) + + image_semseg.convert(carla.ColorConverter.CityScapesPalette) + fps = round(1.0 / snapshot.timestamp.delta_seconds) + + # Draw the display. + draw_image(display, image_rgb) + draw_image(display, image_semseg, blend=True) + display.blit( + font.render( + "% 5d FPS (real)" % clock.get_fps(), True, (255, 255, 255) + ), + (8, 10), + ) + display.blit( + font.render( + "% 5d FPS (simulated)" % fps, True, (255, 255, 255) + ), + (8, 28), + ) + pygame.display.flip() + finally: + for actor in actor_list: + actor.destroy() + + pygame.quit() + env.close() diff --git a/rl_studio/agents/auto_carla/utils.py b/rl_studio/agents/auto_carla/utils.py new file mode 100644 index 000000000..71c1f9f15 --- /dev/null +++ b/rl_studio/agents/auto_carla/utils.py @@ -0,0 +1,763 @@ +import logging +import imutils +import sys + +import cv2 +import numpy as np + + +def format_time(seconds): + """ + Convert the elapsed time from seconds into a human-readable format. + """ + minutes, seconds = divmod(seconds, 60) + hours, minutes = divmod(minutes, 60) + milliseconds = seconds * 1000 + seconds, milliseconds = divmod(milliseconds, 1000) + return "{:02}:{:02}:{:02}.{:03}".format( + int(hours), int(minutes), int(seconds), int(milliseconds) + ) + + +def get_variables_size(): + total_size = 0 + all_vars = { + k: v for k, v in globals().items() if not k.startswith("_") + } # Exclude variables starting with _ + all_vars.update(locals()) # Include local variables + for var_name, var_value in all_vars.items(): + if not var_name.startswith("_"): + size = sys.getsizeof(var_value) + # print(f"Size of {var_name}: {size} bytes") + total_size += size + + return all_vars, total_size + + +def old_get_variables_size(): + """ + Get the size of declared variables in global and local scopes + + using it: + + # Get the dictionary of variable names and sizes + variables_size = get_variables_size() + + # Print the dictionary + for var_name, size in variables_size.items(): + print(f"{var_name}: {size} bytes") + + """ + all_vars = {} + all_vars.update(globals()) # Add global variables + all_vars.update(locals()) # Add local variables + + variables_size = {} + for var_name, var in all_vars.items(): + # Filter out built-in and module variables + if ( + not var_name.startswith("__") + and not hasattr(var, "__module__") + and not var_name.startswith("_") + ): + variables_size[var_name] = sys.getsizeof(var) + return variables_size + + +class Logger: + def __init__(self, log_file): + # Configure the log format + format = "%(asctime)s - %(name)s - %(levelname)s - %(message)s" + logging.basicConfig(level=logging.DEBUG, format=format) + + # File handler + file_handler = logging.FileHandler(log_file) + file_handler.setLevel( + logging.WARNING + ) # Only warnings and errors will be written to the file + file_handler.setFormatter(logging.Formatter(format)) + logging.getLogger().addHandler(file_handler) + + # Remove the console handler + self._remove_console_handler() + + def _remove_console_handler(self): + root_logger = logging.getLogger() + for handler in root_logger.handlers: + if isinstance(handler, logging.StreamHandler): + root_logger.removeHandler(handler) + + def _info(self, message): + logging.info(message) + + def _warning(self, message): + logging.warning(message) + + def _error(self, message): + logging.error(message) + + def _debug(self, message): + logging.debug(message) + + +class Logger_: + """ + shows warning in console + + + """ + + def __init__(self, log_file): + # Configurar el formato de los registros + format = "%(asctime)s - %(name)s - %(levelname)s - %(message)s" + logging.basicConfig(level=logging.DEBUG, format=format) + + # File handler + file_handler = logging.FileHandler(log_file) + file_handler.setLevel( + logging.WARNING + ) # Solo se escribirán advertencias y errores en el archivo + file_handler.setFormatter(logging.Formatter(format)) + logging.getLogger().addHandler(file_handler) + + # Configurar un manipulador para imprimir en la consola + console_handler = logging.StreamHandler() + console_handler.setLevel( + logging.INFO + ) # Solo se imprimirán mensajes de info y debug en la consola + console_handler.setFormatter(logging.Formatter(format)) + logging.getLogger().addHandler(console_handler) + + def _info(self, message): + logging.info(message) + + def _warning(self, message): + logging.warning(message) + + def _error(self, message): + logging.error(message) + + def _debug(self, message): + logging.debug(message) + + +class LoggerAllInOne: + def __init__(self, log_file): + format = "%(asctime)s - %(name)s - %(levelname)s - %(message)s" + logging.basicConfig(filename=log_file, level=logging.DEBUG, format=format) + self.logger = logging.getLogger(__name__) + + def _info(self, message): + self.logger.info(message) + + def _warning(self, message): + self.logger.warning(message) + + def _error(self, message): + self.logger.error(message) + + def _debug(self, message): + self.logger.debug(message) + + +class LoggingHandler: + def __init__(self, log_file): + self.logger = logging.getLogger(__name__) + c_handler = logging.StreamHandler() + f_handler = logging.FileHandler(log_file) + + c_handler.setLevel(logging.DEBUG) + f_handler.setLevel(logging.INFO) + + # Create formatters and add it to handlers + c_format = logging.Formatter("%(name)s - %(levelname)s - %(message)s") + f_format = logging.Formatter( + "[%(levelname)s] - %(asctime)s, filename: %(filename)s, funcname: %(funcName)s, line: %(lineno)s\n messages ---->\n %(message)s" + ) + c_handler.setFormatter(c_format) + f_handler.setFormatter(f_format) + + # Add handlers to the logger + self.logger.addHandler(c_handler) + self.logger.addHandler(f_handler) + + +class AutoCarlaUtils: + def __init__(self): + self.f1 = None + + @staticmethod + def finish_target(current_car_pose, target_pose, max_distance): + """ + working with waypoints + """ + current_car_pose_x = current_car_pose[0] + current_car_pose_y = current_car_pose[1] + + # in case working with waypoints, use next + target_x = target_pose.transform.location.x + target_y = target_pose.transform.location.y + + dist = ((current_car_pose_x - target_x) ** 2) + ( + (current_car_pose_y - target_y) ** 2 + ) + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + + # print(f"{current_car_pose = }") + # print(f"{target_x = }, {target_y = }") + # print(f"{dist = }") + + # print(dist) + if dist < max_distance: + return True, dist + return False, dist + + @staticmethod + def finish_fix_number_target( + current_car_pose, targets_set, target_pose, max_distance + ): + """ + working with tuple 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + """ + current_car_pose_x = current_car_pose[0] + current_car_pose_y = current_car_pose[1] + + target_x = targets_set[target_pose][0] + target_y = targets_set[target_pose][1] + + dist = ((current_car_pose_x - target_x) ** 2) + ( + (current_car_pose_y - target_y) ** 2 + ) + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + + # print(f"{current_car_pose = }") + # print(f"{target_x = }, {target_y = }") + # print(f"{dist = }") + + # print(dist) + if dist < max_distance: + return True, dist + return False, dist + + @staticmethod + def show_image_lines_centers_borders( + name, + img, + x_row, + x, + y, + index_right, + index_left, + centers, + drawing_lines_states, + drawing_numbers_states, + ): + """ + show image RGB with: + x_row lines + + centers points + + center image line + + lane borders points + + heading vertical line + + """ + + window_name = f"{name}" + img = np.array(img) + + ## vertical line in the center of image, showing car position + cv2.line( + img, + (int(img.shape[1] // 2), int(img.shape[0] // 2)), + (int(img.shape[1] // 2), int(img.shape[0])), + # (320, 120), + # (320, 480), + color=(200, 100, 100), + thickness=4, + ) + + ## heading line from upper center point to lower center point + cv2.line( + img, + (centers[0], x_row[0]), + (centers[-1], x_row[-1]), + # (320, 120), + # (320, 480), + color=(0, 0, 255), + thickness=2, + ) + + drawing_lines_states.append(640) + ## vertical lines for states: 5, 7, 8, 16... + for index, _ in enumerate(drawing_lines_states): + cv2.line( + img, + (drawing_lines_states[index], 0), + (drawing_lines_states[index], int(img.shape[0])), + color=(100, 200, 100), + thickness=1, + ) + ## writing number state into lines + cv2.putText( + img, + str(f"{drawing_numbers_states[index]}"), + ( + drawing_lines_states[index] - 15, + 15, + ), ## -15 is the distance to the its left line + cv2.FONT_HERSHEY_SIMPLEX, + 0.4, + (0, 0, 0), + 1, + cv2.LINE_AA, + ) + + for index, _ in enumerate(x_row): + ### HORIZONTAL LINES x_row + cv2.line( + img, + (0, int(x_row[index])), + (int(img.shape[1]), int(x_row[index])), + color=(100, 200, 100), + thickness=1, + ) + + ### Points + cv2.circle( + img, + (int(index_right[index]), x_row[index]), + 5, + # (150, 200, 150), + (0, 0, 255), + 2, + ) + cv2.circle( + img, + (int(index_left[index]), int(x_row[index])), + 4, + # (150, 200, 150), + (0, 0, 255), + 1, + ) + cv2.circle( + img, + (int(centers[index]), int(x_row[index])), + 4, + # (150, 200, 150), + (0, 0, 0), + 1, + ) + + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, img) + cv2.waitKey(1) + + @staticmethod + def show_image_only_right_line( + name, + img, + waitkey, + dist_in_pixels, + ground_truth_pixel_values, + dist_normalized, + states, + x_row, + x, + y, + line_states, + number_states, + ): + """ + shows image with 1 point in center of lane + """ + window_name = f"{name}" + img = np.array(img) + + ## vertical line in the center of image, showing car position + cv2.line( + img, + (int(img.shape[1] // 2), int(img.shape[0] // 2)), + (int(img.shape[1] // 2), int(img.shape[0])), + # (320, 120), + # (320, 480), + color=(200, 100, 100), + thickness=4, + ) + + line_states.append(640) + ## vertical lines for states: 5, 7, 8, 16... + for index, _ in enumerate(line_states): + cv2.line( + img, + (line_states[index], 0), + (line_states[index], int(img.shape[0])), + color=(100, 200, 100), + thickness=1, + ) + ## writing number state into lines + # for index, value in enumerate(number_states): + cv2.putText( + img, + str(f"{number_states[index]}"), + (line_states[index] - 30, 15), + cv2.FONT_HERSHEY_SIMPLEX, + 0.4, + (0, 0, 0), + 1, + cv2.LINE_AA, + ) + + for index, _ in enumerate(x_row): + ### Points + cv2.circle( + img, + ( + int(dist_in_pixels[index]), + int(x_row[index]), + ), + 5, + # (150, 200, 150), + (255, 255, 255), + 2, + ) + cv2.circle( + img, + (int(ground_truth_pixel_values[index]), int(x_row[index])), + 4, + # (150, 200, 150), + (255, 255, 255), + 1, + ) + + cv2.putText( + img, + str( + f"[right_line:{int(dist_in_pixels[index])}]-[dist:{dist_normalized[index]}]" + # f"[dist_norm:{int(centrals_in_pixels[index])}]-[state:{states[index]}]-[dist:{errors[index]}]" + ), + (int(dist_in_pixels[index]), int(x_row[index]) - 5), + cv2.FONT_HERSHEY_SIMPLEX, + 0.4, + # (255, 255, 255), + (255, 255, 255), + 1, + cv2.LINE_AA, + ) + + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, img) + cv2.waitKey(waitkey) + + @staticmethod + def show_image_with_three_points( + name, + img, + waitkey, + dist_in_pixels, + dist_normalized, + states, + x_row, + x, + y, + left_points, + right_points, + ): + """ + shows image with 1 point in center of lane + """ + window_name = f"{name}" + img = np.array(img) + + for index, _ in enumerate(x_row): + ### horizontal line in x_row file + cv2.line( + img, + (0, int(x_row[index])), + (int(img.shape[1]), int(x_row[index])), + color=(100, 200, 100), + thickness=1, + ) + ### vertical line in center of the image + cv2.line( + img, + (int(img.shape[1] // 2), 0), + (int(img.shape[1] // 2), int(img.shape[0])), + color=(100, 200, 100), + thickness=1, + ) + ### left limit vertical line (40%) + cv2.line( + img, + (int(img.shape[1] // 2) - (int((img.shape[1] // 2) * 0.4)), 0), + ( + int(img.shape[1] // 2) - (int((img.shape[1] // 2) * 0.4)), + int(img.shape[0]), + ), + color=(100, 200, 100), + thickness=1, + ) + ### right limit vertical line + cv2.line( + img, + (int(img.shape[1] // 2) + (int((img.shape[1] // 2) * 0.4)), 0), + ( + int(img.shape[1] // 2) + (int((img.shape[1] // 2) * 0.4)), + int(img.shape[0]), + ), + color=(100, 200, 100), + thickness=1, + ) + + ### Points + ## center point + cv2.circle( + img, + (int(dist_in_pixels[index]), int(x_row[index])), + 5, + # (150, 200, 150), + (255, 255, 255), + 2, + ) + # left point + cv2.circle( + img, + (int(left_points[index]), int(x_row[index])), + 5, + # (150, 200, 150), + (255, 255, 255), + 2, + ) + # right point + cv2.circle( + img, + (int(right_points[index]), int(x_row[index])), + 5, + # (150, 200, 150), + (255, 255, 255), + 2, + ) + + cv2.putText( + img, + str( + f"[right_line:{int(dist_in_pixels[index])}]-[state:{states[index]}]-[dist:{dist_normalized[index]}]" + # f"[dist_norm:{int(centrals_in_pixels[index])}]-[state:{states[index]}]-[dist:{errors[index]}]" + ), + (int(dist_in_pixels[index]), int(x_row[index]) - 5), + cv2.FONT_HERSHEY_SIMPLEX, + 0.4, + # (255, 255, 255), + (255, 255, 255), + 1, + cv2.LINE_AA, + ) + + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, img) + cv2.waitKey(waitkey) + + @staticmethod + def show_image_with_center_point( + name, img, waitkey, dist_in_pixels, dist_normalized, states, x_row, x, y + ): + """ + shows image with 1 point in center of lane + """ + window_name = f"{name}" + img = np.array(img) + + for index, _ in enumerate(x_row): + ### horizontal line in x_row file + cv2.line( + img, + (0, int(x_row[index])), + (int(img.shape[1]), int(x_row[index])), + color=(100, 200, 100), + thickness=1, + ) + ### vertical line in center of the image + cv2.line( + img, + (int(img.shape[1] // 2), 0), + (int(img.shape[1] // 2), int(img.shape[0])), + color=(100, 200, 100), + thickness=1, + ) + ### left limit vertical line (40%) + cv2.line( + img, + (int(img.shape[1] // 2) - (int((img.shape[1] // 2) * 0.4)), 0), + ( + int(img.shape[1] // 2) - (int((img.shape[1] // 2) * 0.4)), + int(img.shape[0]), + ), + color=(100, 200, 100), + thickness=1, + ) + ### right limit vertical line + cv2.line( + img, + (int(img.shape[1] // 2) + (int((img.shape[1] // 2) * 0.4)), 0), + ( + int(img.shape[1] // 2) + (int((img.shape[1] // 2) * 0.4)), + int(img.shape[0]), + ), + color=(100, 200, 100), + thickness=1, + ) + + ### Points + cv2.circle( + img, + ( + int(img.shape[1] // 2) + int(dist_in_pixels[index]), + int(x_row[index]), + ), + 5, + # (150, 200, 150), + (255, 255, 255), + 2, + ) + cv2.circle( + img, + (int(img.shape[1] // 2), int(x_row[index])), + 4, + # (150, 200, 150), + (255, 255, 255), + 1, + ) + + cv2.putText( + img, + str( + f"[right_line:{int(dist_in_pixels[index])}]-[state:{states[index]}]-[dist:{dist_normalized[index]}]" + # f"[dist_norm:{int(centrals_in_pixels[index])}]-[state:{states[index]}]-[dist:{errors[index]}]" + ), + (int(dist_in_pixels[index]), int(x_row[index]) - 5), + cv2.FONT_HERSHEY_SIMPLEX, + 0.4, + # (255, 255, 255), + (255, 255, 255), + 1, + cv2.LINE_AA, + ) + + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, img) + cv2.waitKey(waitkey) + + @staticmethod + def show_image_with_centrals( + name, img, waitkey, centrals_in_pixels, centrals_normalized, x_row, x, y + ): + window_name = f"{name}" + img = np.array(img) + + for index, _ in enumerate(x_row): + cv2.putText( + img, + str(f"{int(centrals_in_pixels[index])}"), + ( + (img.shape[1] // 2) + int(centrals_in_pixels[index]) + 20, + int(x_row[index]), + ), + cv2.FONT_HERSHEY_SIMPLEX, + 0.3, + # (255, 255, 255), + (255, 255, 255), + 1, + cv2.LINE_AA, + ) + cv2.putText( + img, + str(f"[{centrals_normalized[index]}]"), + (img.shape[1] - 100, int(x_row[index])), + cv2.FONT_HERSHEY_SIMPLEX, + 0.3, + # (255, 255, 255), + (255, 255, 255), + 1, + cv2.LINE_AA, + ) + cv2.line( + img, + (0, int(x_row[index])), + (int(img.shape[1]), int(x_row[index])), + color=(100, 200, 100), + thickness=1, + ) + cv2.line( + img, + (int(img.shape[1] // 2), 0), + (int(img.shape[1] // 2), int(img.shape[0])), + color=(100, 200, 100), + thickness=1, + ) + + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, img) + cv2.waitKey(waitkey) + + @staticmethod + def show_image(name, img, x, y): + window_name = f"{name}" + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, img) + cv2.waitKey(1) + + @staticmethod + def show_images(name, img, x, y): + window_name = f"{name}" + hori = np.concatenate(img, axis=1) + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, hori) + cv2.waitKey(1) + + @staticmethod + def show_images_tile(name, list_img, x, y): + window_name = f"{name}" + # img_tile = cv2.vconcat([cv2.hconcat(list_h) for list_h in list_img]) + + width_common = 320 + height_common = 240 + # image resizing + # im_list_resize = [ + # cv2.resize( + # np.array(img), + # (width_common, height_common), + # interpolation=cv2.INTER_CUBIC, + # ) + # for img in list_img + # ] + im_list_resize = [ + imutils.resize( + np.array(img), + width=width_common, + ) + for img in list_img + ] + # print(f"{im_list_resize = }") + # for i, value in enumerate(im_list_resize): + # print(f"({i = }") + # print(f"{len(im_list_resize) = }, {type(im_list_resize) = }") + im_list = [img for img in im_list_resize] + # print(f"{len(im_list) = }, {type(im_list) = }") + + im_list_concat = np.concatenate(im_list, axis=1) + # im_list_concat = cv2.hconcat( + # [im_list_resize[0], im_list_resize[1], im_list_resize[2]] + # ) + # im_list_concat = cv2.hconcat([im_list_resize[0], im_list_resize[1]]) + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, im_list_concat) + cv2.waitKey(1) diff --git a/rl_studio/agents/f1/loaders.py b/rl_studio/agents/f1/loaders.py index fbecbc348..7408554c0 100644 --- a/rl_studio/agents/f1/loaders.py +++ b/rl_studio/agents/f1/loaders.py @@ -16,7 +16,7 @@ def __init__(self, config): self.batch_size = config["algorithm"]["ddpg"]["batch_size"] elif config["settings"]["algorithm"] == "dqn": - self.alpha = config["algorithm"]["dqn"]["alpha"] + # self.alpha = config["algorithm"]["dqn"]["alpha"] self.gamma = config["algorithm"]["dqn"]["gamma"] self.epsilon = config["algorithm"]["dqn"]["epsilon"] self.epsilon_discount = config["algorithm"]["dqn"]["epsilon_discount"] @@ -29,8 +29,8 @@ def __init__(self, config): self.minibatch_size = config["algorithm"]["dqn"]["minibatch_size"] self.update_target_every = config["algorithm"]["dqn"]["update_target_every"] self.memory_fraction = config["algorithm"]["dqn"]["memory_fraction"] - self.buffer_capacity = config["algorithm"]["dqn"]["buffer_capacity"] - self.batch_size = config["algorithm"]["dqn"]["batch_size"] + # self.buffer_capacity = config["algorithm"]["dqn"]["buffer_capacity"] + # self.batch_size = config["algorithm"]["dqn"]["batch_size"] elif config["settings"]["algorithm"] == "qlearn": self.alpha = config["algorithm"]["qlearn"]["alpha"] @@ -84,9 +84,47 @@ class LoadGlobalParams: def __init__(self, config): self.stats = {} # epoch: steps - self.states_counter = {} + self.states_actions_counter = {} self.states_reward = {} + self.time_steps = {} + self.time_training_steps = {} self.ep_rewards = [] + + self.im_general_ddpg = { + "episode": [], + "step": [], + "cumulated_reward": [], + "FPS_avg": [], + "FPS_median": [], + "epoch_time": [], + "lane_changed": [], + "distance_to_finish": [], + } + + # this variable is for QLEARN + self.im_general = { + "episode": [], + "step": [], + "cumulated_reward": [], + "epsilon": [], + "FPS_avg": [], + "FPS_median": [], + "epoch_time": [], + "lane_changed": [], + "distance_to_finish": [], + } + self.im_q_table = { + "state": [], + "action": [], + "q_value": [], + } + self.im_state_actions_counter = { + "epoch": [], + "state": [], + "action": [], + "counter": [], + } + self.actions_rewards = { "episode": [], "step": [], @@ -112,6 +150,7 @@ def __init__(self, config): "current_total_training_time": [], } self.settings = config["settings"] + self.station = config["settings"]["station"] self.mode = config["settings"]["mode"] self.task = config["settings"]["task"] self.algorithm = config["settings"]["algorithm"] @@ -507,6 +546,13 @@ def __init__(self, config) -> None: self.actions = config["settings"]["actions"] self.actions_set = config["actions"][self.actions] self.rewards = config["settings"]["rewards"] + self.town = config[self.environment_set][self.env]["town"] + self.waypoints_lane_id = config[self.environment_set][self.env][ + "waypoints_lane_id" + ] + self.town_circuit = config[self.environment_set][self.env]["init_pose"][ + "town_circuit" + ] ##### environment variable self.environment = {} self.environment["agent"] = config["settings"]["agent"] @@ -528,7 +574,6 @@ def __init__(self, config) -> None: self.environment["town"] = config[self.environment_set][self.env]["town"] self.environment["car"] = config[self.environment_set][self.env]["car"] self.environment["weather"] = config[self.environment_set][self.env]["weather"] - self.environment["weather"] = config[self.environment_set][self.env]["weather"] self.environment["traffic_pedestrians"] = config[self.environment_set][ self.env ]["traffic_pedestrians"] @@ -538,8 +583,170 @@ def __init__(self, config) -> None: self.environment["car_lights"] = config[self.environment_set][self.env][ "car_lights" ] + self.environment["save_episodes"] = config[self.environment_set][self.env][ + "save_episodes" + ] + self.environment["save_every_step"] = config[self.environment_set][self.env][ + "save_every_step" + ] + self.environment["filter"] = config[self.environment_set][self.env]["filter"] + self.environment["generation"] = config[self.environment_set][self.env][ + "generation" + ] + self.environment["rolename"] = config[self.environment_set][self.env][ + "rolename" + ] + self.environment["gamma"] = config[self.environment_set][self.env]["gamma"] + self.environment["sync"] = config[self.environment_set][self.env]["sync"] + self.environment["waypoints_meters"] = config[self.environment_set][self.env][ + "waypoints_meters" + ] + self.environment["waypoints_init"] = config[self.environment_set][self.env][ + "waypoints_init" + ] + self.environment["waypoints_target"] = config[self.environment_set][self.env][ + "waypoints_target" + ] + self.environment["waypoints_lane_id"] = config[self.environment_set][self.env][ + "waypoints_lane_id" + ] + self.environment["waypoints_road_id"] = config[self.environment_set][self.env][ + "waypoints_road_id" + ] + self.environment["max_target_waypoint_distance"] = config[self.environment_set][ + self.env + ]["max_target_waypoint_distance"] + self.environment["alternate_pose"] = config[self.environment_set][self.env][ - "alternate_pose" + "init_pose" + ]["alternate_pose"] + self.environment["random_pose"] = config[self.environment_set][self.env][ + "init_pose" + ]["random_pose"] + self.environment["init_pose_number"] = config[self.environment_set][self.env][ + "init_pose" + ]["init_pose_number"] + self.environment["finish_pose_number"] = config[self.environment_set][self.env][ + "init_pose" + ]["finish_pose_number"] + self.environment["start_alternate_pose"] = config["carla_town_positions"][ + self.town + ][self.town_circuit][self.waypoints_lane_id]["init_positions"] + self.environment["finish_alternate_pose"] = config["carla_town_positions"][ + self.town + ][self.town_circuit][self.waypoints_lane_id]["finish_positions"] + + # --------- Image + self.environment["height_image"] = config["agents"][self.agent][ + "camera_params" + ]["height"] + self.environment["width_image"] = config["agents"][self.agent]["camera_params"][ + "width" + ] + self.environment["center_image"] = config["agents"][self.agent][ + "camera_params" + ]["center_image"] + self.environment["image_resizing"] = config["agents"][self.agent][ + "camera_params" + ]["image_resizing"] + self.environment["new_image_size"] = config["agents"][self.agent][ + "camera_params" + ]["new_image_size"] + self.environment["raw_image"] = config["agents"][self.agent]["camera_params"][ + "raw_image" + ] + self.environment["num_regions"] = config["agents"][self.agent]["camera_params"][ + "num_regions" + ] + self.environment["lower_limit"] = config["agents"][self.agent]["camera_params"][ + "lower_limit" + ] + # States + self.environment["states"] = config["settings"]["states"] + self.environment["x_row"] = config["states"][self.states][0] + self.environment["states_entry"] = config["states"]["states_entry"] + + # Actions + self.environment["action_space"] = config["settings"]["actions"] + self.environment["actions"] = config["actions"][self.actions] + + # Rewards + self.environment["reward_function"] = config["settings"]["rewards"] + self.environment["rewards"] = config["rewards"][self.rewards] + self.environment["min_reward"] = config["rewards"][self.rewards]["min_reward"] + + # Algorithm + self.environment["alpha"] = config["algorithm"]["qlearn"]["alpha"] + self.environment["epsilon"] = config["algorithm"]["qlearn"]["epsilon"] + self.environment["epsilon_min"] = config["algorithm"]["qlearn"]["epsilon_min"] + self.environment["gamma"] = config["algorithm"]["qlearn"]["gamma"] + + # CARLA + self.environment["carla_server"] = config["carla"]["carla_server"] + self.environment["carla_client"] = config["carla"]["carla_client"] + + +class LoadEnvVariablesDDPGCarla: + """ + ONLY FOR DDPG algorithm + Creates a new variable 'environment', which contains Carla env values + """ + + def __init__(self, config) -> None: + """environment variable for reset(), step() methods""" + # self.agent = config["settings"]["agent"] + # self.algorithm = config["settings"]["algorithm"] + # self.task = config["settings"]["task"] + # self.framework = config["settings"]["framework"] + self.environment_set = config["settings"]["environment_set"] + self.env = config["settings"]["env"] + self.agent = config["settings"]["agent"] + self.states = config["settings"]["states"] + self.actions = config["settings"]["actions"] + self.actions_set = config["actions"][self.actions] + self.rewards = config["settings"]["rewards"] + self.town = config[self.environment_set][self.env]["town"] + self.waypoints_lane_id = config[self.environment_set][self.env][ + "waypoints_lane_id" + ] + self.town_circuit = config[self.environment_set][self.env]["init_pose"][ + "town_circuit" + ] + ##### environment variable + self.environment = {} + self.environment["agent"] = config["settings"]["agent"] + self.environment["algorithm"] = config["settings"]["algorithm"] + self.environment["task"] = config["settings"]["task"] + self.environment["framework"] = config["settings"]["framework"] + + # Training/inference + self.environment["mode"] = config["settings"]["mode"] + self.environment["retrain_ddpg_tf_actor_model_name"] = config["retraining"][ + "ddpg" + ]["retrain_ddpg_tf_actor_model_name"] + self.environment["retrain_ddpg_tf_critic_model_name"] = config["retraining"][ + "ddpg" + ]["retrain_ddpg_tf_critic_model_name"] + self.environment["inference_ddpg_tf_actor_model_name"] = config["inference"][ + "ddpg" + ]["inference_ddpg_tf_actor_model_name"] + self.environment["inference_ddpg_tf_critic_model_name"] = config["inference"][ + "ddpg" + ]["inference_ddpg_tf_critic_model_name"] + + # Env + self.environment["env"] = config["settings"]["env"] + self.environment["town"] = config[self.environment_set][self.env]["town"] + self.environment["car"] = config[self.environment_set][self.env]["car"] + self.environment["weather"] = config[self.environment_set][self.env]["weather"] + self.environment["traffic_pedestrians"] = config[self.environment_set][ + self.env + ]["traffic_pedestrians"] + self.environment["city_lights"] = config[self.environment_set][self.env][ + "city_lights" + ] + self.environment["car_lights"] = config[self.environment_set][self.env][ + "car_lights" ] self.environment["save_episodes"] = config[self.environment_set][self.env][ "save_episodes" @@ -547,11 +754,178 @@ def __init__(self, config) -> None: self.environment["save_every_step"] = config[self.environment_set][self.env][ "save_every_step" ] - self.environment["init_pose"] = config[self.environment_set][self.env][ + self.environment["filter"] = config[self.environment_set][self.env]["filter"] + self.environment["generation"] = config[self.environment_set][self.env][ + "generation" + ] + self.environment["rolename"] = config[self.environment_set][self.env][ + "rolename" + ] + self.environment["gamma"] = config[self.environment_set][self.env]["gamma"] + self.environment["sync"] = config[self.environment_set][self.env]["sync"] + self.environment["waypoints_meters"] = config[self.environment_set][self.env][ + "waypoints_meters" + ] + self.environment["waypoints_init"] = config[self.environment_set][self.env][ + "waypoints_init" + ] + self.environment["waypoints_target"] = config[self.environment_set][self.env][ + "waypoints_target" + ] + self.environment["waypoints_lane_id"] = config[self.environment_set][self.env][ + "waypoints_lane_id" + ] + self.environment["waypoints_road_id"] = config[self.environment_set][self.env][ + "waypoints_road_id" + ] + self.environment["max_target_waypoint_distance"] = config[self.environment_set][ + self.env + ]["max_target_waypoint_distance"] + + self.environment["alternate_pose"] = config[self.environment_set][self.env][ + "init_pose" + ]["alternate_pose"] + self.environment["random_pose"] = config[self.environment_set][self.env][ + "init_pose" + ]["random_pose"] + self.environment["init_pose_number"] = config[self.environment_set][self.env][ "init_pose" + ]["init_pose_number"] + self.environment["finish_pose_number"] = config[self.environment_set][self.env][ + "init_pose" + ]["finish_pose_number"] + self.environment["start_alternate_pose"] = config["carla_town_positions"][ + self.town + ][self.town_circuit][self.waypoints_lane_id]["init_positions"] + self.environment["finish_alternate_pose"] = config["carla_town_positions"][ + self.town + ][self.town_circuit][self.waypoints_lane_id]["finish_positions"] + + # --------- Image + self.environment["height_image"] = config["agents"][self.agent][ + "camera_params" + ]["height"] + self.environment["width_image"] = config["agents"][self.agent]["camera_params"][ + "width" + ] + self.environment["center_image"] = config["agents"][self.agent][ + "camera_params" + ]["center_image"] + self.environment["image_resizing"] = config["agents"][self.agent][ + "camera_params" + ]["image_resizing"] + self.environment["new_image_size"] = config["agents"][self.agent][ + "camera_params" + ]["new_image_size"] + self.environment["raw_image"] = config["agents"][self.agent]["camera_params"][ + "raw_image" + ] + self.environment["num_regions"] = config["agents"][self.agent]["camera_params"][ + "num_regions" + ] + self.environment["lower_limit"] = config["agents"][self.agent]["camera_params"][ + "lower_limit" + ] + # States + self.environment["states"] = config["settings"]["states"] + self.environment["x_row"] = config["states"][self.states][0] + self.environment["states_entry"] = config["states"]["states_entry"] + + # Actions + self.environment["action_space"] = config["settings"]["actions"] + self.environment["actions"] = config["actions"][self.actions] + + # Rewards + self.environment["reward_function"] = config["settings"]["rewards"] + self.environment["rewards"] = config["rewards"][self.rewards] + self.environment["min_reward"] = config["rewards"][self.rewards]["min_reward"] + + # Algorithm + # Algorithm + self.environment["critic_lr"] = config["algorithm"]["ddpg"]["critic_lr"] + self.environment["actor_lr"] = config["algorithm"]["ddpg"]["actor_lr"] + self.environment["model_name"] = config["algorithm"]["ddpg"]["model_name"] + + self.environment["gamma"] = config["algorithm"]["ddpg"]["gamma"] + self.environment["tau"] = config["algorithm"]["ddpg"]["tau"] + self.environment["std_dev"] = config["algorithm"]["ddpg"]["std_dev"] + self.environment["replay_memory_size"] = config["algorithm"]["ddpg"][ + "replay_memory_size" + ] + self.environment["memory_fraction"] = config["algorithm"]["ddpg"][ + "memory_fraction" + ] + self.environment["buffer_capacity"] = config["algorithm"]["ddpg"][ + "buffer_capacity" + ] + self.environment["batch_size"] = config["algorithm"]["ddpg"]["batch_size"] + + # CARLA + self.environment["carla_server"] = config["carla"]["carla_server"] + self.environment["carla_client"] = config["carla"]["carla_client"] + + +class LoadEnvVariablesDQNCarla: + """ + ONLY FOR DQN algorithm + Creates a new variable 'environment', which contains Carla env values + """ + + def __init__(self, config) -> None: + """environment variable for reset(), step() methods""" + # self.agent = config["settings"]["agent"] + # self.algorithm = config["settings"]["algorithm"] + # self.task = config["settings"]["task"] + # self.framework = config["settings"]["framework"] + self.environment_set = config["settings"]["environment_set"] + self.env = config["settings"]["env"] + self.agent = config["settings"]["agent"] + self.states = config["settings"]["states"] + self.actions = config["settings"]["actions"] + self.actions_set = config["actions"][self.actions] + self.rewards = config["settings"]["rewards"] + self.town = config[self.environment_set][self.env]["town"] + self.waypoints_lane_id = config[self.environment_set][self.env][ + "waypoints_lane_id" + ] + self.town_circuit = config[self.environment_set][self.env]["init_pose"][ + "town_circuit" + ] + ##### environment variable + self.environment = {} + self.environment["agent"] = config["settings"]["agent"] + self.environment["algorithm"] = config["settings"]["algorithm"] + self.environment["task"] = config["settings"]["task"] + self.environment["framework"] = config["settings"]["framework"] + self.environment["circuit_name"] = config["settings"]["env"] + # Training/inference + self.environment["mode"] = config["settings"]["mode"] + self.environment["retrain_dqn_tf_model_name"] = config["retraining"]["dqn"][ + "retrain_dqn_tf_model_name" ] - self.environment["goal_pose"] = config[self.environment_set][self.env][ - "goal_pose" + self.environment["inference_dqn_tf_model_name"] = config["inference"]["dqn"][ + "inference_dqn_tf_model_name" + ] + + # Env + self.environment["env"] = config["settings"]["env"] + self.environment["town"] = config[self.environment_set][self.env]["town"] + self.environment["car"] = config[self.environment_set][self.env]["car"] + self.environment["weather"] = config[self.environment_set][self.env]["weather"] + self.environment["traffic_pedestrians"] = config[self.environment_set][ + self.env + ]["traffic_pedestrians"] + self.environment["city_lights"] = config[self.environment_set][self.env][ + "city_lights" + ] + self.environment["car_lights"] = config[self.environment_set][self.env][ + "car_lights" + ] + self.environment["save_episodes"] = config[self.environment_set][self.env][ + "save_episodes" + ] + self.environment["save_every_step"] = config[self.environment_set][self.env][ + "save_every_step" ] self.environment["filter"] = config[self.environment_set][self.env]["filter"] self.environment["generation"] = config[self.environment_set][self.env][ @@ -577,6 +951,33 @@ def __init__(self, config) -> None: self.environment["waypoints_road_id"] = config[self.environment_set][self.env][ "waypoints_road_id" ] + self.environment["max_target_waypoint_distance"] = config[self.environment_set][ + self.env + ]["max_target_waypoint_distance"] + + self.environment["alternate_pose"] = config[self.environment_set][self.env][ + "init_pose" + ]["alternate_pose"] + self.environment["random_pose"] = config[self.environment_set][self.env][ + "init_pose" + ]["random_pose"] + self.environment["init_pose_number"] = config[self.environment_set][self.env][ + "init_pose" + ]["init_pose_number"] + self.environment["finish_pose_number"] = config[self.environment_set][self.env][ + "init_pose" + ]["finish_pose_number"] + self.environment["start_alternate_pose"] = config["carla_town_positions"][ + self.town + ][self.town_circuit][self.waypoints_lane_id]["init_positions"] + self.environment["finish_alternate_pose"] = config["carla_town_positions"][ + self.town + ][self.town_circuit][self.waypoints_lane_id]["finish_positions"] + + # ----- agent + self.environment["target_vel"] = config["agents"][self.agent]["targets"][ + "target_vel" + ] # --------- Image self.environment["height_image"] = config["agents"][self.agent][ @@ -606,6 +1007,7 @@ def __init__(self, config) -> None: # States self.environment["states"] = config["settings"]["states"] self.environment["x_row"] = config["states"][self.states][0] + self.environment["states_entry"] = config["states"]["states_entry"] # Actions self.environment["action_space"] = config["settings"]["actions"] @@ -617,10 +1019,181 @@ def __init__(self, config) -> None: self.environment["min_reward"] = config["rewards"][self.rewards]["min_reward"] # Algorithm - self.environment["alpha"] = config["algorithm"]["qlearn"]["alpha"] - self.environment["epsilon"] = config["algorithm"]["qlearn"]["epsilon"] - self.environment["epsilon_min"] = config["algorithm"]["qlearn"]["epsilon_min"] - self.environment["gamma"] = config["algorithm"]["qlearn"]["gamma"] + # self.environment["alpha"] = config["algorithm"]["dqn"]["alpha"] + self.environment["gamma"] = config["algorithm"]["dqn"]["gamma"] + self.environment["epsilon"] = config["algorithm"]["dqn"]["epsilon"] + self.environment["epsilon_discount"] = config["algorithm"]["dqn"][ + "epsilon_discount" + ] + self.environment["epsilon_min"] = config["algorithm"]["dqn"]["epsilon_min"] + self.environment["model_name"] = config["algorithm"]["dqn"]["model_name"] + self.environment["replay_memory_size"] = config["algorithm"]["dqn"][ + "replay_memory_size" + ] + self.environment["min_replay_memory_size"] = config["algorithm"]["dqn"][ + "min_replay_memory_size" + ] + self.environment["minibatch_size"] = config["algorithm"]["dqn"][ + "minibatch_size" + ] + self.environment["memory_fraction"] = config["algorithm"]["dqn"][ + "memory_fraction" + ] + # self.environment["buffer_capacity"] = config["algorithm"]["dqn"][ + # "buffer_capacity" + # ] + # self.environment["batch_size"] = config["algorithm"]["dqn"]["batch_size"] + + # CARLA + self.environment["carla_server"] = config["carla"]["carla_server"] + self.environment["carla_client"] = config["carla"]["carla_client"] + self.environment["traffic_manager_port"] = config["carla"][ + "traffic_manager_port" + ] + + +class LoadEnvVariablesSB3Carla: + """ + ONLY FOR Stable-Baselines3 algorithms + Creates a new variable 'environment', which contains Carla env values + """ + + def __init__(self, config) -> None: + """environment variable for reset(), step() methods""" + # self.agent = config["settings"]["agent"] + # self.algorithm = config["settings"]["algorithm"] + # self.task = config["settings"]["task"] + # self.framework = config["settings"]["framework"] + self.environment_set = config["settings"]["environment_set"] + self.env = config["settings"]["env"] + self.agent = config["settings"]["agent"] + self.states = config["settings"]["states"] + self.actions = config["settings"]["actions"] + self.actions_set = config["actions"][self.actions] + self.rewards = config["settings"]["rewards"] + self.town = config[self.environment_set][self.env]["town"] + self.waypoints_lane_id = config[self.environment_set][self.env][ + "waypoints_lane_id" + ] + self.town_circuit = config[self.environment_set][self.env]["init_pose"][ + "town_circuit" + ] + ##### environment variable + self.environment = {} + self.environment["agent"] = config["settings"]["agent"] + self.environment["algorithm"] = config["settings"]["algorithm"] + self.environment["task"] = config["settings"]["task"] + self.environment["framework"] = config["settings"]["framework"] + self.environment["circuit_name"] = config["settings"]["env"] + + # Training/inference + self.environment["mode"] = config["settings"]["mode"] + + # Env + self.environment["env"] = config["settings"]["env"] + self.environment["town"] = config[self.environment_set][self.env]["town"] + self.environment["car"] = config[self.environment_set][self.env]["car"] + self.environment["weather"] = config[self.environment_set][self.env]["weather"] + self.environment["traffic_pedestrians"] = config[self.environment_set][ + self.env + ]["traffic_pedestrians"] + self.environment["city_lights"] = config[self.environment_set][self.env][ + "city_lights" + ] + self.environment["car_lights"] = config[self.environment_set][self.env][ + "car_lights" + ] + self.environment["save_episodes"] = config[self.environment_set][self.env][ + "save_episodes" + ] + self.environment["save_every_step"] = config[self.environment_set][self.env][ + "save_every_step" + ] + self.environment["filter"] = config[self.environment_set][self.env]["filter"] + self.environment["generation"] = config[self.environment_set][self.env][ + "generation" + ] + self.environment["rolename"] = config[self.environment_set][self.env][ + "rolename" + ] + self.environment["gamma"] = config[self.environment_set][self.env]["gamma"] + self.environment["sync"] = config[self.environment_set][self.env]["sync"] + self.environment["waypoints_meters"] = config[self.environment_set][self.env][ + "waypoints_meters" + ] + self.environment["waypoints_init"] = config[self.environment_set][self.env][ + "waypoints_init" + ] + self.environment["waypoints_target"] = config[self.environment_set][self.env][ + "waypoints_target" + ] + self.environment["waypoints_lane_id"] = config[self.environment_set][self.env][ + "waypoints_lane_id" + ] + self.environment["waypoints_road_id"] = config[self.environment_set][self.env][ + "waypoints_road_id" + ] + self.environment["max_target_waypoint_distance"] = config[self.environment_set][ + self.env + ]["max_target_waypoint_distance"] + + self.environment["alternate_pose"] = config[self.environment_set][self.env][ + "init_pose" + ]["alternate_pose"] + self.environment["random_pose"] = config[self.environment_set][self.env][ + "init_pose" + ]["random_pose"] + self.environment["init_pose_number"] = config[self.environment_set][self.env][ + "init_pose" + ]["init_pose_number"] + self.environment["finish_pose_number"] = config[self.environment_set][self.env][ + "init_pose" + ]["finish_pose_number"] + self.environment["start_alternate_pose"] = config["carla_town_positions"][ + self.town + ][self.town_circuit][self.waypoints_lane_id]["init_positions"] + self.environment["finish_alternate_pose"] = config["carla_town_positions"][ + self.town + ][self.town_circuit][self.waypoints_lane_id]["finish_positions"] + + # --------- Image + self.environment["height_image"] = config["agents"][self.agent][ + "camera_params" + ]["height"] + self.environment["width_image"] = config["agents"][self.agent]["camera_params"][ + "width" + ] + self.environment["center_image"] = config["agents"][self.agent][ + "camera_params" + ]["center_image"] + self.environment["image_resizing"] = config["agents"][self.agent][ + "camera_params" + ]["image_resizing"] + self.environment["new_image_size"] = config["agents"][self.agent][ + "camera_params" + ]["new_image_size"] + self.environment["raw_image"] = config["agents"][self.agent]["camera_params"][ + "raw_image" + ] + self.environment["num_regions"] = config["agents"][self.agent]["camera_params"][ + "num_regions" + ] + self.environment["lower_limit"] = config["agents"][self.agent]["camera_params"][ + "lower_limit" + ] + # States + self.environment["states"] = config["settings"]["states"] + self.environment["x_row"] = config["states"][self.states][0] + self.environment["states_entry"] = config["states"]["states_entry"] + + # Actions + self.environment["action_space"] = config["settings"]["actions"] + self.environment["actions"] = config["actions"][self.actions] + + # Rewards + self.environment["reward_function"] = config["settings"]["rewards"] + self.environment["rewards"] = config["rewards"][self.rewards] + self.environment["min_reward"] = config["rewards"][self.rewards]["min_reward"] # CARLA self.environment["carla_server"] = config["carla"]["carla_server"] diff --git a/rl_studio/agents/f1/train_followline_ddpg_f1_gazebo_tf.py b/rl_studio/agents/f1/train_followline_ddpg_f1_gazebo_tf.py index af6839454..cc338f8f0 100644 --- a/rl_studio/agents/f1/train_followline_ddpg_f1_gazebo_tf.py +++ b/rl_studio/agents/f1/train_followline_ddpg_f1_gazebo_tf.py @@ -163,8 +163,8 @@ def main(self): ) render_params( task=self.global_params.task, - v=action[0][0], # for continuous actions - w=action[0][1], # for continuous actions + # v=action[0][0], # for continuous actions + # w=action[0][1], # for continuous actions episode=episode, step=step, state=state, @@ -273,6 +273,7 @@ def main(self): ac_agent, self.global_params, self.algoritmhs_params, + self.environment.environment, cumulated_reward, episode, "BESTLAP", @@ -301,6 +302,7 @@ def main(self): ac_agent, self.global_params, self.algoritmhs_params, + self.environment.environment, cumulated_reward, episode, "FINISHTIME", @@ -341,6 +343,7 @@ def main(self): ac_agent, self.global_params, self.algoritmhs_params, + self.environment.environment, cumulated_reward, episode, "BATCH", diff --git a/rl_studio/agents/frameworks_type.py b/rl_studio/agents/frameworks_type.py index dbd020637..6a38a4dc9 100644 --- a/rl_studio/agents/frameworks_type.py +++ b/rl_studio/agents/frameworks_type.py @@ -4,3 +4,4 @@ class FrameworksType(Enum): TF = "TensorFlow" PYTORCH = "Pytorch" + STABLE_BASELINES3 = "sb3" diff --git a/rl_studio/agents/utilities/plot_stats.py b/rl_studio/agents/utilities/plot_stats.py new file mode 100644 index 000000000..77059a019 --- /dev/null +++ b/rl_studio/agents/utilities/plot_stats.py @@ -0,0 +1,55 @@ +import os +import time + +import matplotlib +import matplotlib.pyplot as plt +import pandas as pd + + +class StatsDataFrame: + def save_dataframe_stats( + self, environment, outdir, stats, q_table=False, sac=False + ): + os.makedirs(f"{outdir}", exist_ok=True) + + if q_table: + file_csv = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_QTABLE_Circuit-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.csv" + file_excel = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_QTABLE_Circuit-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.xlsx" + elif sac: + file_csv = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_SAC_Circuit-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.csv" + file_excel = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_SAC_Circuit-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.xlsx" + else: + file_csv = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.csv" + file_excel = f"{outdir}/{time.strftime('%Y%m%d')}_Circuit-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.xlsx" + + df = pd.DataFrame(stats) + #df.to_csv(file_csv, mode="w", index=False, header=None) + + # with pd.ExcelWriter(file_excel, mode="a") as writer: + # df.to_excel(writer) + df.to_excel(file_excel) + with pd.ExcelWriter( + file_excel, engine="openpyxl", mode="a", if_sheet_exists="replace" + ) as writer: + df.to_excel(writer) + + # writer.close() + # df.close() + + +class MetricsPlot: + def __init__(self, outdir, line_color="blue", **args): + """ + renders graphs of intrinsic and extrinsic metrics + Args: + + """ + self.outdir = outdir + self.line_color = line_color + + # styling options + matplotlib.rcParams["toolbar"] = "None" + plt.style.use("ggplot") + fig = plt.gcf().canvas.set_window_title("simulation_graph") + + # def plot(self, env): diff --git a/rl_studio/agents/utilities/stats_qlearn_plotter.py b/rl_studio/agents/utilities/stats_qlearn_plotter.py new file mode 100644 index 000000000..f6803d2df --- /dev/null +++ b/rl_studio/agents/utilities/stats_qlearn_plotter.py @@ -0,0 +1,354 @@ +import argparse +import glob +import os +import random +import sys +import time + +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd + + +def plot_ie_metrics(file): + df = pd.read_excel(file, usecols=[1, 2, 3, 4, 5, 6, 7, 8, 9]) + print(df) + + ### creating in one canvas + fig, ax = plt.subplots(2, 2, figsize=(20, 12)) + ax[0, 0].plot( + df["episode"], + df["cumulated_reward"], + color="blue", + linewidth=2, + linestyle="-", + label="rewards", + ) # Plot some data on the (implicit) axes. + ax[0, 0].plot( + df["episode"], + df["step"], + color="orange", + linewidth=2, + linestyle="--", + label="steps", + ) # Plot some data on the (implicit) axes. + ax[0, 0].set_xlabel("episodes") + ax[0, 0].set_ylabel("value") + ax[0, 0].set_title("Rewards/steps per epoch") + ax[0, 0].legend() + + ax[0, 1].set_yscale("log") + ax[0, 1].plot( + df["episode"], + df["epsilon"], + color="green", + linewidth=1, + linestyle="-", + label="epsilon", + ) # Plot some data on the (implicit) axes. + ax[0, 1].set_xlabel("episodes") + ax[0, 1].set_ylabel("epsilon value [0 - 0.99]") + ax[0, 1].set_title("epsilon per epoch") + + ax[1, 0].plot( + df["episode"], + df["distance_to_finish"], + color="red", + linewidth=2, + linestyle="-", + label="distance", + ) # Plot some data on the (implicit)axes. + ax[1, 0].set_xlabel("episodes") + ax[1, 0].set_ylabel("distance") + ax[1, 0].set_title("Distance to Finish circuit") + + ax[1, 1].plot( + df["episode"], + df["epoch_time"], + color="black", + linewidth=2, + linestyle="-", + label="time", + ) # Plot some data on the (implicit) axes. + ax[1, 1].set_xlabel("episodes") + ax[1, 1].set_ylabel("time") + ax[1, 1].set_title("time in every epoch") + + # saving + file_saved = f"{time.strftime('%Y%m%d-%H%M%S')}_ie_metrics" + plt.savefig(f"{file_saved}.png", dpi=600) + plt.savefig(f"{file_saved}.jpg", dpi=600) + + ### individuals ##### + # a, b = np.polyfit(df["episode"], df["cumulated_reward"], deg=1) + a, b, c = np.polyfit(df["episode"], df["cumulated_reward"], deg=2) + # a, b, c, d = np.polyfit(df["episode"], df["cumulated_reward"], deg=3) + # y_est = a * df["episode"] + b + y_est = a * np.square(df["episode"]) + b * df["episode"] + c + # y_est = ( + # a * (df["episode"] ** 3) + b * np.square(df["episode"]) + c * df["episode"] + d + # ) + y_err = df["episode"].std() * np.sqrt( + 1 / len(df["episode"]) + + (df["episode"] - df["episode"].mean()) ** 2 + / np.sum((df["episode"] - df["episode"].mean()) ** 2) + ) + + fig1, axs1 = plt.subplots(1, 1, sharey=True, tight_layout=True) + axs1.plot(df["episode"], y_est, color="red", linestyle="-") + axs1.fill_between(df["episode"], y_est - y_err, y_est + y_err, alpha=0.2) + + axs1.plot( + df["episode"], + df["cumulated_reward"], + color="blue", + linewidth=2, + linestyle="-", + label="rewards", + ) # Plot some data on the (implicit) axes. + axs1.plot( + df["episode"], + df["step"], + color="orange", + linewidth=2, + linestyle="--", + label="steps", + ) + # Plot some data on the (implicit) axes. + axs1.set_xlabel("episodes") + axs1.set_ylabel("value") + axs1.set_title("Rewards/steps per epoch") + axs1.legend() + + file_rewards = f"{time.strftime('%Y%m%d-%H%M%S')}_rewards_steps" + plt.savefig(f"{file_rewards}.png", dpi=600) + plt.savefig(f"{file_rewards}.jpg", dpi=600) + + # epsilon + fig2, axs2 = plt.subplots(1, 1, sharey=True, tight_layout=True) + axs2.set_yscale("log") + axs2.plot( + df["episode"], + df["epsilon"], + color="green", + linewidth=1, + linestyle="-", + label="epsilon", + ) # Plot some data on the (implicit) axes. + axs2.set_xlabel("episodes") + axs2.set_ylabel("epsilon value [0 - 0.99]") + axs2.set_title("epsilon per epoch") + + file_epsilon = f"{time.strftime('%Y%m%d-%H%M%S')}_epsilon" + plt.savefig(f"{file_epsilon}.png", dpi=600) + plt.savefig(f"{file_epsilon}.jpg", dpi=600) + + # distance to finish + fig3, axs3 = plt.subplots(1, 1, sharey=True, tight_layout=True) + axs3.plot( + df["episode"], + df["distance_to_finish"], + color="red", + linewidth=2, + linestyle="-", + label="distance", + ) # Plot some data on the (implicit)axes. + axs3.set_xlabel("episodes") + axs3.set_ylabel("distance") + axs3.set_title("Distance to Finish circuit") + + file_dist = f"{time.strftime('%Y%m%d-%H%M%S')}_distance" + plt.savefig(f"{file_dist}.png", dpi=600) + plt.savefig(f"{file_dist}.jpg", dpi=600) + + # epoch time + fig4, axs4 = plt.subplots(1, 1, sharey=True, tight_layout=True) + axs4.plot( + df["episode"], + df["epoch_time"], + color="black", + linewidth=2, + linestyle="-", + label="epoch time", + ) # Plot some data on the (implicit) axes. + axs4.set_xlabel("episodes") + axs4.set_ylabel("time") + axs4.set_title("time in every epoch") + + file_time = f"{time.strftime('%Y%m%d-%H%M%S')}_time" + plt.savefig(f"{file_time}.png", dpi=600) + plt.savefig(f"{file_time}.jpg", dpi=600) + + # lane changed + fig5, axs5 = plt.subplots(1, 1, sharey=True, tight_layout=True) + axs5.plot( + df["episode"], + df["lane_changed"], + color="green", + linewidth=2, + linestyle="-", + label="lane changed", + ) # Plot some data on the (implicit) axes. + axs5.set_xlabel("episodes") + axs5.set_ylabel("lane changed") + axs5.set_title("Lane changed in every epoch") + + file_lane_changed = f"{time.strftime('%Y%m%d-%H%M%S')}_lane_changed" + plt.savefig(f"{file_lane_changed}.png", dpi=600) + plt.savefig(f"{file_lane_changed}.jpg", dpi=600) + + +def plot_sac_metrics(file): + df_sac = pd.read_excel(file, usecols=[1, 2, 3, 4]) + print(df_sac) + + ### STATES + num_states = 5 + list_states = [ + df_sac[df_sac["state"] == i][["counter"]].sum() for i in range(0, num_states) + ] + + states_df = pd.DataFrame(list_states, columns=["state", "counter"]) + # print(states_df) + states_df["state"] = states_df.index + # print(states_df) + + fig1, axs1 = plt.subplots(1, 1, figsize=(10, 6), sharey=True, tight_layout=True) + axs1.bar(states_df["state"], states_df["counter"], color="dodgerblue") + axs1.set_xlabel("states") + axs1.set_ylabel("frequency") + axs1.set_title("Histogram of States") + axs1.set_xticks(states_df["state"]) + + file_states = f"{time.strftime('%Y%m%d-%H%M%S')}_histogram_states" + plt.savefig(f"{file_states}.png", dpi=600) + plt.savefig(f"{file_states}.jpg", dpi=600) + + #### ACTIONS + num_actions = 5 + list_actions = [ + df_sac[df_sac["action"] == i][["counter"]].sum() for i in range(0, num_actions) + ] + + actions_df = pd.DataFrame(list_actions, columns=["action", "counter"]) + # print(states_df) + actions_df["action"] = actions_df.index + # print(actions_df) + + fig2, axs2 = plt.subplots(1, 1, sharey=True, tight_layout=True) + axs2.bar( + actions_df["action"], + actions_df["counter"], + tick_label=[ + "0", + "1", + "2", + "3", + "4", + ], # change in function of line 228, var num_actions + color="teal", + ) + axs2.set_xlabel("actions") + axs2.set_ylabel("frequency") + axs2.set_title("Histogram of Actions") + axs2.set_xticks(actions_df["action"]) + + file_actions = f"{time.strftime('%Y%m%d-%H%M%S')}_histogram_actions" + plt.savefig(f"{file_actions}.png", dpi=600) + plt.savefig(f"{file_actions}.jpg", dpi=600) + + +def plot_qtable(file): + df_qtable = pd.read_excel(file, usecols=[1, 2, 3]) + # df_sac = pd.read_excel("asac.xlsx") + print(df_qtable) + dataf_size = df_qtable.shape[0] + + actions = [0, 1, 2] + states = [i for i in range(0, 8)] + z_pos = np.where(df_qtable["q_value"] >= 0, df_qtable["q_value"], 0) + z_neg = np.where(df_qtable["q_value"] < 0, df_qtable["q_value"], 0) + fig = plt.figure(figsize=(10, 10)) + + ax1 = fig.add_subplot(111, projection="3d") + # ax1.bar3d(df_qtable['state'], df_qtable['action'], np.zeros(dataf_size), np.ones(dataf_size), np.ones(dataf_size), df_qtable['q_value']) + ax1.bar3d(df_qtable["state"], df_qtable["action"], 0.1, 0.1, 0.1, z_pos) + ax1.bar3d(df_qtable["state"], df_qtable["action"], 0.1, 0.1, 0.1, z_neg) + # for y in enumerate(actions): + # ax1.bar(df_qtable['state'], df_qtable['q_value'], zs=y, zdir='y') + + ax1.set_xlabel("states") + ax1.set_ylabel("actions") + ax1.set_zlabel("value") + ax1.set_title("Q-Table Values", fontsize=14, fontweight="bold") + + ax1.set_yticks(actions[::-1]) + ax1.set_xticks(states) + + file_qtable = f"{time.strftime('%Y%m%d-%H%M%S')}_qtable" + plt.savefig(f"{file_qtable}.png", dpi=600) + plt.savefig(f"{file_qtable}.jpg", dpi=600) + + +############################################################## + + +def generate_plotter(args): + # print(f"{args=}") + if args.type == "ie_metrics": + # df = pd.read_excel(args.file) + # print(f"{df=}") + """ + ie_metrics means for intrinsic and extrinsic metrics + """ + plot_ie_metrics(args.file) + + elif args.type == "sac": + print("SAC") + """sac stands for states and actions counter""" + plot_sac_metrics(args.file) + else: + print("q_table") + """q-table""" + plot_qtable(args.file) + + +def main(): + argparser = argparse.ArgumentParser(description="Plot Q-Learn Carla Stats") + argparser.add_argument( + "-t", + "--type", + required=True, + help="type of data: q-table, sac, ie_metrics", + ) + argparser.add_argument( + "-f", + "--file", + required=True, + help="load data file", + ) + + args = argparser.parse_args() + # args2 = vars(argparser.parse_args()) + + print(f"{args=}") + # print(f"{args2=}") + + try: + generate_plotter(args) + + except KeyboardInterrupt: + print("\nCancelled by user. Bye!") + + +if __name__ == "__main__": + """ + How to use it: + + options: + -t, --type: you have 3 options, q-table, sac, ie_metrics + -f, --file: depending on -t option, you have to choose the correct file + + """ + + main() diff --git a/rl_studio/agents/utils.py b/rl_studio/agents/utils.py index 7010ba446..baed77f94 100644 --- a/rl_studio/agents/utils.py +++ b/rl_studio/agents/utils.py @@ -1,3 +1,4 @@ +from collections import Counter, OrderedDict from datetime import datetime, timedelta import logging import os @@ -21,12 +22,12 @@ def __init__(self, log_file): c_handler = logging.StreamHandler() f_handler = logging.FileHandler(log_file) - c_handler.setLevel(logging.INFO) + c_handler.setLevel(logging.DEBUG) f_handler.setLevel(logging.INFO) # Create formatters and add it to handlers c_format = logging.Formatter("%(name)s - %(levelname)s - %(message)s") - f_format: Formatter = logging.Formatter( + f_format = logging.Formatter( "[%(levelname)s] - %(asctime)s, filename: %(filename)s, funcname: %(funcName)s, line: %(lineno)s\n messages ---->\n %(message)s" ) c_handler.setFormatter(c_format) @@ -77,7 +78,6 @@ class Bcolors: def print_messages(*args, **kwargs): - print(f"\n\t{Bcolors.OKCYAN}====>\t{args[0]}:{Bcolors.ENDC}") for key, value in kwargs.items(): print(f"{Bcolors.OKBLUE}[INFO] {key} = {value}{Bcolors.ENDC}") @@ -92,7 +92,36 @@ def print_dictionary(dic): def render_params(**kwargs): font = cv2.FONT_HERSHEY_SIMPLEX - canvas = np.zeros((400, 400, 3), dtype="uint8") + canvas = np.zeros((570, 500, 3), dtype="uint8") + # blue = (255, 0, 0) + # green = (0, 255, 0) + # red = (0, 0, 255) + white = (255, 255, 255) + # white_darkness = (200, 200, 200) + i = 10 + for key, value in kwargs.items(): + cv2.putText( + canvas, + str(f"{key}: {value}"), + (20, i + 25), + font, + 0.5, + white, + 1, + cv2.LINE_AA, + ) + i += 25 + + window_name = "Control Board" + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, 0, 0) # Move it to (40,30) + cv2.imshow(window_name, canvas) + cv2.waitKey(100) + + +def render_params_left_bottom(**kwargs): + font = cv2.FONT_HERSHEY_SIMPLEX + canvas = np.zeros((750, 600, 3), dtype="uint8") # blue = (255, 0, 0) # green = (0, 255, 0) # red = (0, 0, 255) @@ -112,7 +141,10 @@ def render_params(**kwargs): ) i += 25 - cv2.imshow("Control Board", canvas) + window_name = "Stats Board" + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, 0, 400) # Move it to (40,30) + cv2.imshow(window_name, canvas) cv2.waitKey(100) @@ -122,18 +154,53 @@ def save_dataframe_episodes(environment, outdir, aggr_ep_rewards, actions_reward """ os.makedirs(f"{outdir}", exist_ok=True) - file_csv = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{environment['circuit_name']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.csv" - file_excel = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{environment['circuit_name']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.xlsx" + file_csv = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_town-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.csv" + file_excel = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_town-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.xlsx" + + df = pd.DataFrame(aggr_ep_rewards) + # df.to_csv(file_csv, mode="a", index=False, header=None) + df.to_excel(file_excel) + + if actions_rewards is not None: + file_npy = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_town-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.npy" + np.save(file_npy, actions_rewards) + + +def save_carla_dataframe_episodes( + environment, outdir, aggr_ep_rewards, actions_rewards=None +): + """ + We save info every certains epochs in a dataframe and .npy format to export or manage + """ + os.makedirs(f"{outdir}", exist_ok=True) + + file_csv = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.csv" + file_excel = f"{outdir}/{time.strftime('%Y%m%d')}_Circuit-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.xlsx" df = pd.DataFrame(aggr_ep_rewards) - df.to_csv(file_csv, mode="a", index=False, header=None) + # df.to_csv(file_csv, mode="a", index=False, header=None) + + # with pd.ExcelWriter(file_excel, mode="a") as writer: + # df.to_excel(writer) df.to_excel(file_excel) if actions_rewards is not None: - file_npy = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{environment['circuit_name']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.npy" + file_npy = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.npy" np.save(file_npy, actions_rewards) +def save_carla_latency(environment, outdir, latency): + total = 0 + for key, value in latency.items(): + total += value + avg = total / len(latency) + + os.makedirs(f"{outdir}", exist_ok=True) + file_excel = f"{outdir}/{time.strftime('%Y%m%d')}_Circuit-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}.xlsx" + df = pd.DataFrame(latency) + df.to_excel(file_excel) + + def save_best_episode( global_params, cumulated_reward, @@ -218,7 +285,7 @@ def save_batch(episode, step, start_time_epoch, start_time, global_params, env_p global_params.aggr_ep_rewards["max"].append(max_reward) global_params.aggr_ep_rewards["min"].append(min_reward) global_params.aggr_ep_rewards["epoch_training_time"].append( - (datetime.now() - start_time_epoch).total_seconds() + (time.time() - start_time_epoch) ) global_params.aggr_ep_rewards["total_training_time"].append( (datetime.now() - start_time).total_seconds() diff --git a/rl_studio/algorithms/ddpg.py b/rl_studio/algorithms/ddpg.py index d289145bb..bed2f0d18 100644 --- a/rl_studio/algorithms/ddpg.py +++ b/rl_studio/algorithms/ddpg.py @@ -1,6 +1,7 @@ import time import numpy as np +from keras import backend as K import tensorflow as tf from tensorflow.keras import Input, Model, layers from tensorflow.keras.callbacks import TensorBoard @@ -12,10 +13,11 @@ Activation, Flatten, Rescaling, + Layer, ) from tensorflow.keras.models import Sequential, load_model from tensorflow.keras.optimizers import Adam - +from tensorflow.keras.utils import get_custom_objects # Sharing GPU gpus = tf.config.experimental.list_physical_devices("GPU") @@ -25,7 +27,6 @@ # Own Tensorboard class class ModifiedTensorBoard(TensorBoard): - # Overriding init to set initial step and writer (we want one log file for all .fit() calls) def __init__(self, **kwargs): super().__init__(**kwargs) @@ -140,7 +141,12 @@ def record(self, obs_tuple): else: self.state_buffer[index] = obs_tuple[0] - self.next_state_buffer[index] = obs_tuple[3] + obs_tuple_3 = np.array(obs_tuple[3]) + # self.next_state_buffer[index] = obs_tuple[3] + self.next_state_buffer[index] = np.reshape( + obs_tuple_3, obs_tuple_3.shape[0] + ) + if self.action_space == "continuous": self.action_buffer[index] = [ obs_tuple[1][0][i] for i in range(len(obs_tuple[1][0])) @@ -161,7 +167,6 @@ def update( reward_batch, next_state_batch, ): - with tf.GradientTape() as tape: target_actions = actor_critic.target_actor(next_state_batch, training=True) y = reward_batch + gamma * actor_critic.target_critic( @@ -263,7 +268,6 @@ def learn(self, actor_critic, gamma): class DDPGAgent: def __init__(self, config, action_space_size, observation_space_values, outdir): - self.ACTION_SPACE_SIZE = action_space_size self.OBSERVATION_SPACE_VALUES = observation_space_values if config["states"] == "image": @@ -392,11 +396,10 @@ def load_inference_model(self, models_dir, config): # Based on rate `tau`, which is much less than one. @tf.function def update_target(self, target_weights, weights, tau): - for (a, b) in zip(target_weights, weights): + for a, b in zip(target_weights, weights): a.assign(b * tau + a * (1 - tau)) def policy(self, state, noise_object, action_space): - if action_space == "continuous": return self.policy_continuous_actions(state, noise_object) else: @@ -479,25 +482,67 @@ def policy_continuous_actions(self, state, noise_object): sampled_actions = tf.squeeze(self.actor_model(state)) noise = noise_object() # Adding noise to action + + print(f"{sampled_actions.numpy()[0] = } and {sampled_actions.numpy()[1] = }") + sampled_actions = sampled_actions.numpy() + noise # we can discretized the actions values with round(,0) - legal_action_v = round( - np.clip(sampled_actions[0], self.V_LOWER_BOUND, self.V_UPPER_BOUND), 1 + # legal_action_v = round( + # np.clip(sampled_actions[0], self.V_LOWER_BOUND, self.V_UPPER_BOUND), 3 + # ) + # legal_action_w = round( + # np.clip(sampled_actions[1], self.W_RIGHT_BOUND, self.W_LEFT_BOUND), 3 + # ) + + print( + f"sampled action + noise : {sampled_actions[0] = } and {sampled_actions[1] = }" + ) + legal_action_v = np.clip( + sampled_actions[0], self.V_LOWER_BOUND, self.V_UPPER_BOUND ) - legal_action_w = round( - np.clip(sampled_actions[1], self.W_RIGHT_BOUND, self.W_LEFT_BOUND), 1 + legal_action_w = np.clip( + sampled_actions[1], self.W_RIGHT_BOUND, self.W_LEFT_BOUND ) + print(f"{legal_action_v = } and {legal_action_w = }") + legal_action = np.array([legal_action_v, legal_action_w]) return [np.squeeze(legal_action)] + def XXX_get_actor_model_image_continuous_actions(self): + # inputs = Input(shape=self.OBSERVATION_SPACE_VALUES) + model = Sequential() + # Agregar capas al modelo + model.add( + Flatten(input_shape=self.OBSERVATION_SPACE_VALUES) + ) # Capa para aplanar la imagen 32x32 + model.add( + Dense(64, activation="relu") + ) # Capa completamente conectada con 64 neuronas y función de activación ReLU + model.add( + Dense(32, activation="relu") + ) # Capa completamente conectada con 32 neuronas y función de activación ReLU + model.add( + Dense(2, activation="linear") + ) # Capa completamente conectada con 1 neurona y función de activación sigmoid para el rango [0, 1]) + + # Compilar el modelo + model.compile(optimizer="adam", loss="mse", metrics=["accuracy"]) + return model + def get_actor_model_image_continuous_actions(self): # inputShape = (96, 128, 3) inputs = Input(shape=self.OBSERVATION_SPACE_VALUES) - v_branch = self.build_branch_images(inputs, "v_output") - w_branch = self.build_branch_images(inputs, "w_output") + # v_branch = self.build_branch_images(inputs, "v_output") + # w_branch = self.build_branch_images(inputs, "w_output") + v_branch = self.v_branch_nn(inputs, "v_output") + # v_branch = self.v_branch_nn_custom_activation(inputs, "v_output") + w_branch = self.w_branch_nn(inputs, "w_output") + + # normalizing into the actions range v_branch = abs(v_branch) * self.V_UPPER_BOUND w_branch = w_branch * self.W_LEFT_BOUND + # create the model using our input (the batch of images) and # two separate outputs -- model = Model( @@ -508,9 +553,89 @@ def get_actor_model_image_continuous_actions(self): # return the constructed network architecture return model + def v_branch_nn_custom_activation(self, inputs, action_name): + """linear velocity""" + neuron1 = 64 # 32, 64, 128 + neuron2 = 32 # 64, 128, 256 + last_init = tf.random_uniform_initializer(minval=-0.01, maxval=0.01) + + # get_custom_objects().update({'custom_activation': Activation(self.k_relu_max1)}) + # K.register_activation('my_activation', self.k_relu_max1) + + x = Rescaling(1.0 / 255)(inputs) + x = Conv2D(neuron1, (3, 3), padding="same")(x) + # x = Conv2D(32, (3, 3), padding="same")(inputs) + x = Activation(CustomActivation())(x) + x = MaxPooling2D(pool_size=(3, 3))(x) + x = Dropout(0.25)(x) + + x = Conv2D(neuron2, (3, 3), padding="same")(x) + x = Activation(CustomActivation())(x) + x = MaxPooling2D(pool_size=(2, 2))(x) + x = Dropout(0.25)(x) + + x = Flatten()(x) + x = Dense(neuron2)(x) + + x = Dense(1, activation=CustomActivation(), kernel_initializer=last_init)(x) + x = Activation(CustomActivation(), name=action_name)(x) + + return x + + def v_branch_nn(self, inputs, action_name): + """linear velocity""" + neuron1 = 64 # 32, 64, 128 + neuron2 = 32 # 64, 128, 256 + last_init = tf.random_uniform_initializer(minval=-0.01, maxval=0.01) + x = Rescaling(1.0 / 255)(inputs) + x = Conv2D(neuron1, (3, 3), padding="same")(x) + # x = Conv2D(32, (3, 3), padding="same")(inputs) + x = Activation("relu")(x) + x = MaxPooling2D(pool_size=(3, 3))(x) + x = Dropout(0.25)(x) + + x = Conv2D(neuron2, (3, 3), padding="same")(x) + x = Activation("relu")(x) + x = MaxPooling2D(pool_size=(2, 2))(x) + x = Dropout(0.25)(x) + + x = Flatten()(x) + x = Dense(neuron2)(x) + + x = Dense(1, activation="sigmoid", kernel_initializer=last_init)(x) + x = Activation("sigmoid", name=action_name)(x) + + return x + + def w_branch_nn(self, inputs, action_name): + """angular velocity""" + neuron1 = 64 # 32, 64, 128 + neuron2 = 32 # 64, 128, 256 + last_init = tf.random_uniform_initializer(minval=-0.01, maxval=0.01) + x = Rescaling(1.0 / 255)(inputs) + x = Conv2D(neuron1, (3, 3), padding="same")(x) + # x = Conv2D(32, (3, 3), padding="same")(inputs) + x = Activation("relu")(x) + x = MaxPooling2D(pool_size=(3, 3))(x) + x = Dropout(0.25)(x) + + x = Conv2D(neuron2, (3, 3), padding="same")(x) + x = Activation("relu")(x) + x = MaxPooling2D(pool_size=(2, 2))(x) + x = Dropout(0.25)(x) + + x = Flatten()(x) + x = Dense(neuron2)(x) + + x = Dense(1, activation="tanh", kernel_initializer=last_init)(x) + x = Activation("tanh", name=action_name)(x) + + return x + def build_branch_images(self, inputs, action_name): - neuron1 = 32 # 32, 64, 128 - neuron2 = 64 # 64, 128, 256 + """works in Gazebo""" + neuron1 = 256 # 32, 64, 128 + neuron2 = 256 # 64, 128, 256 last_init = tf.random_uniform_initializer(minval=-0.01, maxval=0.01) x = Rescaling(1.0 / 255)(inputs) x = Conv2D(neuron1, (3, 3), padding="same")(x) @@ -635,8 +760,8 @@ def get_actor_model_sp_continuous_actions(self): def build_branch(self, inputs, action_name): last_init = tf.random_uniform_initializer(minval=-0.01, maxval=0.01) # inputs = layers.Input(shape=(self.OBSERVATION_SPACE_VALUES)) - x = Dense(128, activation="relu")(inputs) # 8, 16, 32 neurons - x = Dense(128, activation="relu")(x) # 8, 16, 32 neurons + x = Dense(16, activation="relu")(inputs) # 8, 16, 32 neurons + x = Dense(16, activation="relu")(x) # 8, 16, 32 neurons x = Dense(1, activation="tanh", kernel_initializer=last_init)(x) x = Activation("tanh", name=action_name)(x) @@ -670,3 +795,30 @@ def get_critic_model_sp_continuous_actions(self): model.compile(loss="mse", optimizer=Adam(0.005)) return model + + +def relu_max1(x): + return tf.minimum(tf.maximum(x, 0), 1) + + +@tf.function +def relu_sigmoid(x): + return tf.nn.relu(x) * tf.sigmoid(x) + + +def tanh_relu(x): + return tf.tanh(x) * tf.nn.relu(x) + + +class CustomActivation(layers.Layer): + def __init__(self): + super(CustomActivation, self).__init__() + self.custom_activation = relu_max1 + + def call(self, inputs): + # return tf.minimum(tf.maximum(inputs, 0), 1) + return self.custom_activation(inputs) + + +def k_relu_max1(x): + return K.min(K.max(x, 0), 1) diff --git a/rl_studio/algorithms/dqn_keras.py b/rl_studio/algorithms/dqn_keras.py index b7f1da05b..6db11d8dc 100644 --- a/rl_studio/algorithms/dqn_keras.py +++ b/rl_studio/algorithms/dqn_keras.py @@ -5,7 +5,7 @@ import numpy as np import tensorflow as tf -from tensorflow.keras import Input, Model, layers +from tensorflow.keras import Input, Model, layers, models, layers, optimizers from tensorflow.keras.callbacks import TensorBoard from tensorflow.keras.layers import ( Dense, @@ -17,6 +17,8 @@ Convolution2D, ZeroPadding2D, Rescaling, + Masking, + SimpleRNN, ) from tensorflow.keras.models import Sequential, load_model from tensorflow.keras.optimizers import Adam, RMSprop @@ -66,7 +68,445 @@ def update_stats(self, **stats): self.writer.flush() +class ComplexReplayMemoryGenerator: + def __init__(self, replay_memory, batch_size=32): + """ + Initializes a generator for efficient batch sampling from a replay memory. + + Args: + replay_memory (list): The replay memory containing transitions. + batch_size (int, optional): The number of transitions to sample in each batch. Defaults to 32. + """ + + self.replay_memory = replay_memory + self.batch_size = batch_size + + # Calculate the number of batches based on replay memory size and batch size + self.num_batches = len(self.replay_memory) // batch_size + + def __iter__(self): + """ + Yields batches of transitions from the replay memory. + + Yields: + list: A list of `batch_size` transitions, where each transition is a tuple of + (current_state, action, reward, next_state, done). + """ + + indices = list(range(len(self.replay_memory))) # Create a list of all indices + + while True: + if not indices: # Handle case when all indices have been used + indices = list(range(len(self.replay_memory))) # Reset indices + + # Select a random batch of indices + batch_indices = random.sample(indices, self.batch_size) + + # Remove used indices to avoid duplicates in future batches + for index in batch_indices: + indices.remove(index) + + # Yield the batch of transitions + yield [self.replay_memory[i] for i in batch_indices] + + +class ReplayMemoryGenerator: + def __init__(self, replay_memory, minibatch_size): + self.replay_memory = replay_memory + self.minibatch_size = minibatch_size + + def __iter__(self): + while True: + batch = random.sample(self.replay_memory, self.minibatch_size) + yield batch + + +######################################################################## +# +# DQN Carla +######################################################################## + + class DQN: + + def __init__( + self, + environment, + algorithm_params, + actions_size, + state_size, + outdir, + global_params, + ): + + self.ACTION_SIZE = actions_size + self.STATE_SIZE = state_size + # self.OBSERVATION_SPACE_SHAPE = config.OBSERVATION_SPACE_SHAPE + print(f"\n\tIn DQN class --> {self.ACTION_SIZE =} and {self.STATE_SIZE =}") + + # DQN settings + self.REPLAY_MEMORY_SIZE = ( + algorithm_params.replay_memory_size + ) # How many last steps to keep for model training + self.MIN_REPLAY_MEMORY_SIZE = ( + algorithm_params.min_replay_memory_size + ) # Minimum number of steps in a memory to start training + self.MINIBATCH_SIZE = ( + algorithm_params.minibatch_size + ) # How many steps (samples) to use for training + self.UPDATE_TARGET_EVERY = ( + algorithm_params.update_target_every + ) # Terminal states (end of episodes) + self.MODEL_NAME = algorithm_params.model_name + self.DISCOUNT = algorithm_params.gamma # gamma: min 0 - max 1 + + self.state_space = global_params.states + + # load pretrained model for continuing training (not inference) + if environment["mode"] == "retraining": + print("---------------------- entry load retrained model") + print(f"{outdir}/{environment['retrain_dqn_tf_model_name']}") + # load pretrained actor and critic models + dqn_retrained_model = f"{outdir}/{environment['retrain_dqn_tf_model_name']}" + self.model = load_model(dqn_retrained_model, compile=True) + self.target_model = load_model(dqn_retrained_model, compile=True) + + else: + # main model + # # gets trained every step + if global_params.states == "image": + self.model = self.get_model_conv2D() + # Target model this is what we .predict against every step + self.target_model = self.get_model_conv2D() + self.target_model.set_weights(self.model.get_weights()) + else: + # print( + # f"\n\tDQN class, creating model get_model_simplified_perception()..." + # ) + self.model = self.get_model_simplified_perception() + # Target model this is what we .predict against every step + self.target_model = self.get_model_simplified_perception() + self.target_model.set_weights(self.model.get_weights()) + + # An array with last n steps for training + self.replay_memory = deque(maxlen=self.REPLAY_MEMORY_SIZE) + + # Custom tensorboard object + self.tensorboard = ModifiedTensorBoard( + log_dir=f"{global_params.logs_tensorboard_dir}/{algorithm_params.model_name}-{time.strftime('%Y%m%d-%H%M%S')}" + ) + + # Used to count when to update target network with main network's weights + self.target_update_counter = 0 + + def get_model_simplified_perception_ERROR(self): + """ + RECURRENT NN WITH INPUT VARIABLE AND + A MASK FOR MISSING VALUES (WHICH COME IN VALUES SUCH AS -1 OR 0 + Lane Detectors not always return the same lenght information. IN A PREVIOUS STEP + WE TRANSFOR MISSING VALUES INTO -1, 0 OR ANYTHING ELSE + """ + + ## mask_value = -1 + # model + model = Sequential() + # Masking layer for missing values (-1) + model.add(Masking(mask_value=-1, input_shape=(None, 1))) + # RNN recurrent layer with N units + model.add(Dense(units=16, activation="relu")) + model.add(Dense(units=16, activation="relu")) + # Relu only releases positive values + model.add(Dense(units=self.ACTION_SIZE, activation="linear")) + + # model.compile(optimizer="adam", loss="mse", metrics=["accuracy"]) + + model.compile(optimizer=Adam(0.005), loss="mse", metrics=["accuracy"]) + + return model + + def get_model_simplified_perception_RNN(self): + """ + RECURRENT NN WITH INPUT VARIABLE AND + A MASK FOR MISSING VALUES (WHICH COME IN VALUES SUCH AS -1 OR 0 + Lane Detectors not always return the same lenght information. IN A PREVIOUS STEP + WE TRANSFOR MISSING VALUES INTO -1, 0 OR ANYTHING ELSE + """ + + ## mask_value = -1 + # model + model = Sequential() + # Masking layer for missing values (-1) + model.add(Masking(mask_value=-1, input_shape=(None, 1))) + # RNN recurrent layer with N units + model.add(SimpleRNN(units=32, return_sequences=True)) + model.add(SimpleRNN(units=32)) + # Dense layer with relu activation + model.add(Dense(units=32, activation="relu")) + # Relu only releases positive values + model.add(Dense(units=self.ACTION_SIZE, activation="linear")) + + # model.compile(optimizer="adam", loss="mse", metrics=["accuracy"]) + + model.compile(optimizer=Adam(0.005), loss="mse", metrics=["accuracy"]) + + return model + + def get_model_simplified_perception(self): + """ + mask = -1 + dynamic input + + """ + + neurons1 = 32 + neurons2 = 32 + loss = "mse" + optimizing = 0.005 + + inputs = layers.Input(shape=(self.STATE_SIZE,)) + masked_inputs = layers.Masking(mask_value=-1)( + inputs + ) # Masking layer for handling -1 values + out = layers.Dense(neurons1, activation="relu")(masked_inputs) + out = layers.Dense(neurons2, activation="relu")(out) + outputs = layers.Dense(self.ACTION_SIZE, activation="linear")(out) + model = models.Model(inputs, outputs) + model.compile(loss=loss, optimizer=optimizers.Adam(learning_rate=optimizing)) + return model + + def get_model_simplified_perception_very_simple(self): + """ + simple model with 2 layers. Using for Simplified Perception + No accept -1 + """ + neurons1 = 16 # 32, 64, 256, 400... + neurons2 = 16 # 32, 64, 256, 300... + loss = "mse" + optimizing = 0.005 + + inputs = layers.Input(shape=(self.STATE_SIZE)) + out = layers.Dense(neurons1, activation="relu")(inputs) + out = layers.Dense(neurons2, activation="relu")(out) + outputs = layers.Dense(self.ACTION_SIZE, activation="linear")(out) + model = tf.keras.Model(inputs, outputs) + model.compile(loss=loss, optimizer=Adam(optimizing)) + return model + + """ + def create_model_no_image(self): + model = Sequential() + model.add( + Dense( + 20, input_shape=(2,) + self.OBSERVATION_SPACE_SHAPE, activation="relu" + ) + ) + model.add(Flatten()) + model.add(Dense(18, activation="relu")) + model.add(Dense(10, activation="relu")) + model.add(Dense(self.ACTION_SPACE_SIZE, activation="linear")) + model.compile(loss="mse", optimizer="adam", metrics=["accuracy"]) + return model + """ + + def get_model_conv2D(self): + print(f"self.STATE_SIZE:{self.STATE_SIZE}") + model = Sequential() + # model.add(Conv2D(256, (3, 3), input_shape=(2,) + self.OBSERVATION_SPACE_SHAPE)) + # model.add(Conv2D(256, (3, 3), input_shape=(None,) + self.STATE_SIZE)) + model.add(Rescaling(1./255, input_shape=self.STATE_SIZE)) + + model.add(Conv2D(256, (3, 3), input_shape=self.STATE_SIZE)) + model.add(Activation("relu")) + model.add(MaxPooling2D(pool_size=(2, 2))) + model.add(Dropout(0.2)) + model.add(Conv2D(256, (3, 3))) + model.add(Activation("relu")) + model.add(MaxPooling2D(pool_size=(2, 2))) + model.add(Dropout(0.2)) + model.add(Flatten(input_shape=self.STATE_SIZE)) + model.add(Dense(64)) + model.add(Dense(self.ACTION_SIZE, activation="linear")) + model.compile( + loss="mse", optimizer=Adam(learning_rate=0.001), metrics=["accuracy"] + ) + return model + + def get_model_conv2D__(self): + last_init = tf.random_uniform_initializer(minval=-0.01, maxval=0.01) + # inputs = Input(shape=self.STATE_SIZE) + inputs = Input(shape=(None,) + self.STATE_SIZE) + x = Rescaling(1.0 / 255)(inputs) + x = Conv2D(32, (3, 3), padding="same")(x) + # x = Conv2D(32, (3, 3), padding="same")(inputs) + x = Activation("relu")(x) + x = MaxPooling2D(pool_size=(3, 3), padding="same")(x) + x = Dropout(0.25)(x) + + x = Conv2D(64, (3, 3), padding="same")(x) + x = Activation("relu")(x) + x = MaxPooling2D(pool_size=(2, 2), padding="same")(x) + x = Dropout(0.25)(x) + + x = Flatten()(x) + x = Dense(64)(x) + + x = Dense(self.ACTION_SIZE, activation="tanh", kernel_initializer=last_init)(x) + # x = Activation("tanh", name=action_name)(x) + model = Model(inputs=inputs, outputs=x, name="conv2D") + model.compile( + loss="mse", optimizer=Adam(learning_rate=0.001), metrics=["accuracy"] + ) + return model + + def update_replay_memory(self, transition): + self.replay_memory.append(transition) + + def get_qs(self, state): + if self.state_space == "image": + # return self.model.predict(np.array(state).reshape(-1, *state.shape) / 255)[0] + return self.model.predict(np.array(state).reshape(-1, *state.shape))[0] + else: + # print(f"\n\n{self.model.predict(state)[0] = }, {self.model.predict(state) = }") + # return self.model.predict(np.array([state]) / 255) + return self.model.predict(np.array([state])) + + # return self.model.predict(tf.convert_to_tensor(state)) + + def choose_action(self, state, epsilon): + if np.random.random() > epsilon: + # print(f"\n\tin Training For loop -----> {epsilon =}") + return np.argmax(self.get_qs(state)) + else: + # Get random action + return np.random.randint(0, self.ACTION_SIZE) + + # Trains main network every step during episode + def train(self, terminal_state, step): + + # Start training only if certain number of samples is already saved + if len(self.replay_memory) < self.MIN_REPLAY_MEMORY_SIZE: + return + + # Get a minibatch of random samples from memory replay table + minibatch = random.sample(self.replay_memory, self.MINIBATCH_SIZE) + # Get current states from minibatch, then query NN model for Q values + # current_states = np.array([transition[0] for transition in minibatch]) / 255 + current_states = np.array([transition[0] for transition in minibatch]) + current_qs_list = self.model.predict(current_states) + + # Get future states from minibatch, then query NN model for Q values + # When using target network, query it, otherwise main network should be queried + # new_current_states = np.array([transition[3] for transition in minibatch]) / 255 + new_current_states = np.array([transition[3] for transition in minibatch]) + future_qs_list = self.target_model.predict(new_current_states) + + X = [] # thats the image input + y = [] # thats the label or action to take + + # current_qs = np.zeros((len(current_qs_list), 2)) + # Now we need to enumerate our batches + for index, ( + current_state, + action, + reward, + new_current_state, + done, + ) in enumerate(minibatch): + + # If not a terminal state, get new q from future states, otherwise set it to 0 + # almost like with Q Learning, but we use just part of equation here + if not done: + max_future_q = np.max(future_qs_list[index]) + new_q = reward + self.DISCOUNT * max_future_q + else: + new_q = reward + + # Update Q value for given state + try: + current_qs = current_qs_list[index] + # current_qs = current_qs_list[action] + # current_qs = [] + # print(f"\n\t{current_qs_list[index] =}") + # print(f"\n\t{len(current_qs_list[index]) =}") + # current_qs.append(current_qs_list[index]) + current_qs[action] = new_q + # print(f"\n\tin DQN train ---> {current_qs =}") + # print(f"\n\tin DQN train ---> {len(current_qs) =}") + # print(f"\n\tin DQN train ---> {new_q =}") + # print(f"\n\tin DQN train ---> {index =}") + # print(f"\n\tin DQN train ---> {action =}") + # print(f"\n\tin DQN train ---> {current_qs[action] =}") + except Exception as e: + print(f"\n\tError {e}") + print(f"\n\tin DQN train ---> {len(minibatch) =}") + print(f"\n\tin DQN train ---> {len(new_current_states) =}") + print(f"\n\tin DQN train ---> {len(future_qs_list) =}") + print(f"\n\tin DQN train ---> {len(current_qs_list) =}") + print(f"\n\tin DQN train ---> {type(current_qs_list) =}") + print(f"\n\tin DQN train ---> {current_qs_list[index] =}") + + print(f"\n\tin DQN train ---> {current_qs =}") + print(f"\n\tin DQN train ---> {type(current_qs) =}") + print(f"\n\tin DQN train ---> {len(current_qs) =}") + print(f"\n\tin DQN train ---> {new_q =}") + print(f"\n\tin DQN train ---> {index =}") + print(f"\n\tin DQN train ---> {action =}") + print(f"\n\tin DQN train ---> {current_qs[action] =}") + print(f"In breakpoint press (c)ontinue, (q)uit, (n)ext line, (s)") + breakpoint() + # break + + # print(f"\n\t{len(current_state) =}") + # print(f"\n\t{current_state =}") + # print(f"\n\t{current_qs_list[index] =}") + # print(f"\n\t{current_qs =}") + # print(f"\n\t{len(current_qs) =}") + # And append to our training data + X.append(current_state) # image + y.append(current_qs) # q_value which is Action to take + # y.append(current_qs[action]) # q_value which is Action to take + + # Fit on all samples as one batch, log only on terminal state + self.model.fit( + # np.array(X) / 255, # FOR IMAGES + np.array(X), + np.array(y), + batch_size=self.MINIBATCH_SIZE, + verbose=0, + shuffle=False, + callbacks=[self.tensorboard] if terminal_state else None, + ) + + # Update target network counter every episode + if terminal_state: + self.target_update_counter += 1 + + # If counter reaches set value, update target network with weights of main network + if self.target_update_counter > self.UPDATE_TARGET_EVERY: + self.target_model.set_weights(self.model.get_weights()) + self.target_update_counter = 0 + + def load_inference_model(self, models_dir, config): + """ """ + path_inference_model = f"{models_dir}/{config['inference_dqn_tf_model_name']}" + inference_model = load_model(path_inference_model, compile=False) + # critic_inference_model = load_model(path_critic_inference_model, compile=False) + + return inference_model + + +######################################################################## +# +# DQN based on PythonProgramming.net (Harrison) +######################################################################## + + +class DQN_PP: + """ + this version is based on Python Programming where is developed in other case but well functionning + WORKS ONLY WITH IMAGES, NORMALIZED TO 255 VALUE + """ + def __init__( self, environment, algorithm, actions_size, state_size, outdir, global_params ): @@ -74,6 +514,7 @@ def __init__( self.ACTION_SIZE = actions_size self.STATE_SIZE = state_size # self.OBSERVATION_SPACE_SHAPE = config.OBSERVATION_SPACE_SHAPE + print(f"\n{self.ACTION_SIZE =} and {self.STATE_SIZE =}") # DQN settings self.REPLAY_MEMORY_SIZE = ( @@ -129,9 +570,7 @@ def __init__( def load_inference_model(self, models_dir, config): """ """ - path_inference_model = ( - f"{models_dir}/{config['inference_dqn_tf_model_name']}" - ) + path_inference_model = f"{models_dir}/{config['inference_dqn_tf_model_name']}" inference_model = load_model(path_inference_model, compile=False) # critic_inference_model = load_model(path_critic_inference_model, compile=False) @@ -223,7 +662,10 @@ def get_qs(self, state): 0 ] else: - return self.model.predict(state)[0] + # print(f"\n\n{self.model.predict(state)[0] = }, {self.model.predict(state) = }") + return self.model.predict(np.array([state]) / 255) + + # return self.model.predict(tf.convert_to_tensor(state)) # Trains main network every step during episode def train(self, terminal_state, step): diff --git a/rl_studio/algorithms/qlearn.py b/rl_studio/algorithms/qlearn.py index f8284ffa0..ff0924627 100644 --- a/rl_studio/algorithms/qlearn.py +++ b/rl_studio/algorithms/qlearn.py @@ -3,6 +3,7 @@ import random import time +from memory_profiler import profile import numpy as np @@ -141,10 +142,15 @@ def __init__(self, actions, epsilon=0.99, alpha=0.8, gamma=0.9): self.alpha = alpha # discount constant self.gamma = gamma # discount factor self.actions = actions + print(f"{self.gamma = } and {self.alpha = }") def getQValues(self, state, action): return self.q.get((state, action), 0.0) + def learn(self, state1, action1, reward, state2): + maxqnew = max([self.getQValues(state2, a) for a in self.actions]) + self.learnQ(state1, action1, reward, reward + self.gamma * maxqnew) + def learnQ(self, state, action, reward, value): """ Q-learning: @@ -184,9 +190,6 @@ def selectAction(self, state, return_q=False): return action, q return action - def learn(self, state1, action1, reward, state2): - maxqnew = max([self.getQValues(state2, a) for a in self.actions]) - self.learnQ(state1, action1, reward, reward + self.gamma * maxqnew) def reset(self): self.state = self.np_random.uniform(low=-0.05, high=0.05, size=(4,)) @@ -212,12 +215,27 @@ def inference(self, state, return_q=False): return action def load_pickle_model(self, file_path): - qlearn_file = open(file_path, "rb") self.q = pickle.load(qlearn_file) + def load_table(self, file): + self.q_table = np.load(file) + def load_np_model(self, file): - self.q = np.load(file) + self.q = np.load(file, allow_pickle=True) + + def save_qtable_pickle( + self, environment, outdir, qlearn, cumulated_reward, episode, step, epsilon + ): + os.makedirs(f"{outdir}", exist_ok=True) + # Q TABLE PICKLE + # base_file_name = "_actions_set:_{}_epsilon:_{}".format(settings.actions_set, round(qlearn.epsilon, 2)) + base_file_name = f"{outdir}/{time.strftime('%Y%m%d-%H%M%S')}_Circuit-{environment['town']}_States-{environment['states']}_Actions-{environment['action_space']}_Rewards-{environment['reward_function']}_epsilon-{round(epsilon,3)}_epoch-{episode}_step-{step}_reward-{int(cumulated_reward)}" + file_dump = open( + f"{base_file_name}_QTABLE.pkl", + "wb", + ) + pickle.dump(qlearn.q, file_dump) def save_model( self, @@ -279,7 +297,6 @@ def save_model( # np.save(np_file, qtable) def load_qmodel_actionsmodel(self, file_path, actions_path): - qlearn_file = open(file_path, "rb") actions_file = open(actions_path, "rb") @@ -314,15 +331,17 @@ def select_action(self, state): q_list = [self.q_table.get((state, a), 0.0) for a in self.actions] # print(f"\n{q_list = }") max_q = max(q_list) - # print(f"{max_q}") + # print(f"{max_q = }") count_max = q_list.count(max_q) - # print(f"{count_max}") + # print(f"{count_max= }") + best_index = [index for index, value in enumerate(q_list) if value == max_q] + if count_max > 1: - best = [i for i in range(len(q_list)) if q_list[i] == max_q] - max_q = random.choice(best) - # print(f"{len(self.actions)=}") - if np.random.random() > self.epsilon: - action = q_list.index(max_q) + # best = [i for i in range(len(q_list)) if q_list[i] == max_q] + action = random.choice(best_index) + elif np.random.random() > self.epsilon: + # action = max_q + action = random.choice(best_index) else: action = np.random.randint(0, len(self.actions)) @@ -330,6 +349,7 @@ def select_action(self, state): return action + # @profile def learn(self, state, action, reward, next_state): """ Two ways of similar Q-Learn eq., and we choose 1): @@ -338,7 +358,7 @@ def learn(self, state, action, reward, next_state): 2) Q(s, a) = Q(s,a) + alpha * (reward(s,a) + gamma * Q(s',a) - Q(s,a)) """ - # print(f"q_table al entrar al learn() = {self.q_table}") + # print(f"init q_table in learn() = {self.q_table}") # print(f"{state = }") # print(f"{next_state = }") @@ -361,7 +381,7 @@ def learn(self, state, action, reward, next_state): self.q_table[(state, action)] = ( 1 - self.alpha ) * current_q + self.alpha * (reward + self.gamma * max_q_new) - # print(f"q_table al salir del learn() {self.q_table = }") + # print(f"output q_table learn() {self.q_table = }") def inference(self, state): return np.argmax(self.q_table[state]) @@ -408,7 +428,6 @@ def __init__( self.actions_len = actions_len def select_action(self, state): - # print(f"in selec_action()") # print(f"qlearn.q_table = {self.q_table}") # print(f"len qlearn.q_table = {len(self.q_table)}") diff --git a/rl_studio/algorithms/utils.py b/rl_studio/algorithms/utils.py index e2236e0f0..a3d8c59a1 100644 --- a/rl_studio/algorithms/utils.py +++ b/rl_studio/algorithms/utils.py @@ -7,21 +7,26 @@ def save_actorcritic_model( - agent, global_params, algoritmhs_params, environment, cumulated_reward, episode, text + agent, + global_params, + algoritmhs_params, + environment, + cumulated_reward, + episode, + text, ): - agent.actor_model.save( f"{global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_{algoritmhs_params.model_name}_{text}_ACTOR" - f"Circuit-{environment['circuit_name']}_" + f"town-{environment['town']}_" f"States-{environment['states']}_" f"Actions-{environment['action_space']}_" f"BATCH_Rewards-{environment['reward_function']}_" f"MaxReward-{int(cumulated_reward)}_" f"Epoch-{episode}" - ) + ) agent.critic_model.save( f"{global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_{algoritmhs_params.model_name}_{text}_CRITIC" - f"Circuit-{environment['circuit_name']}_" + f"town-{environment['town']}_" f"States-{environment['states']}_" f"Actions-{environment['action_space']}_" f"BATCH_Rewards-{environment['reward_function']}_" @@ -32,20 +37,19 @@ def save_actorcritic_model( # save model in h5 format agent.actor_model.save( f"{global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_{algoritmhs_params.model_name}_{text}_ACTOR" - f"Circuit-{environment['circuit_name']}_" + f"town-{environment['town']}_" f"States-{environment['states']}_" f"Actions-{environment['action_space']}_" f"BATCH_Rewards-{environment['reward_function']}_" f"MaxReward-{int(cumulated_reward)}_" f"Epoch-{episode}.h5" - ) + ) agent.critic_model.save( f"{global_params.models_dir}/{time.strftime('%Y%m%d-%H%M%S')}_{algoritmhs_params.model_name}_{text}_CRITIC" - f"Circuit-{environment['circuit_name']}_" + f"town-{environment['town']}_" f"States-{environment['states']}_" f"Actions-{environment['action_space']}_" f"BATCH_Rewards-{environment['reward_function']}_" f"MaxReward-{int(cumulated_reward)}_" f"Epoch-{episode}.h5" ) - diff --git a/rl_studio/config/config_inference_followlane_qlearn_carla.yaml b/rl_studio/config/config_inference_followlane_qlearn_carla.yaml new file mode 100644 index 000000000..fc81a8500 --- /dev/null +++ b/rl_studio/config/config_inference_followlane_qlearn_carla.yaml @@ -0,0 +1,346 @@ +settings: + mode: inference # training, retraining, inference + task: follow_lane_carla # follow_line_gazebo, follow_lane_gazebo + algorithm: qlearn # qlearn + simulator: carla # openai, carla, gazebo, sumo + environment_set: carla_environments # gazebo_environments, carla_environments + env: follow_lane # Town01, simple, nurburgring, montreal, curves, simple_laser, manual, autoparking + agent: auto_carla # f1, autoparkingRL, auto_carla, mountain_car, robot_mesh, cartpole, turtlebot + actions: carla_simple # simple, medium, hard, test, autoparking_simple + states: sp4 # sp1 (simplified perception with 1 point), sp3 (simplified perception with 3 points), spn (simplified perception with n points) + rewards: follow_right_lane_only_center # + framework: _ + total_episodes: 50 + training_time: 3 + models_dir: "./checkpoints" + logs_dir: "./logs" + metrics_dir: "./metrics" + recorder_carla_dir: "./recorders" + +ros: + ros_master_uri: "11311" + gazebo_master_uri: "11345" + +carla: + carla_server: localhost + carla_client: 2000 + +retraining: + qlearn: + retrain_qlearn_model_name: + +inference: + qlearn: + #inference_qlearn_model_name: "20230709_QTABLE_Town04_sp4_5states_5actions_Rewards_only_center_20kmh.pkl" + #inference_qlearn_model_name: "20230627-161436_Circuit-Town04_Opt_States-sp4_Actions-carla_simple_Rewards-follow_right_lane_only_center_epsilon-0.234_epoch-227_step-301_reward-3010_QTABLE.pkl" + #inference_qlearn_model_name: "qtable_amano_5states_5actions_sp4.pkl" + #inference_qlearn_model_name: "20230713-092859_Circuit-Town04_Opt_States-sp4_Actions-carla_simple_Rewards-follow_right_lane_only_center_epsilon-0.891_epoch-63_step-301_reward-3010_QTABLE.pkl" + inference_qlearn_model_name: "20230715-171404_Circuit-Town04_Opt_States-sp4_Actions-carla_simple_Rewards-rewards_sigmoid_only_right_line_epsilon-0.779_epoch-181_step-301_reward-2987_QTABLE.pkl" +algorithm: + qlearn: + alpha: 0.7 + epsilon: 0.95 + epsilon_min: 0.05 + gamma: 0.9 + +agents: + auto_carla: + camera_params: + width: 640 + height: 480 + center_image: 320 + raw_image: False + image_resizing: 100 + new_image_size: 32 + num_regions: 7 + lower_limit: 220 + +states: # only values allowed: [10, 20, 30, 60, 80, 100, 130, 180, 200, 230] + sp1: + 0: [100] + sp4: + 0: [30, 40, 50, 60] #[30, 40, 50, 60] #[30, 60, 100, 120] + sp5: + 0: [3, 5, 10, 15, 20] + spn: + 0: [50, 120, 180] + states_entry: one_only_right_line # one_only_right_line, one_between_right_left_lines + +actions: + carla_simple: + 0: [0.45, -0.03] #turn left + 1: [0.55, 0.0] #straight forward + 2: [0.45, 0.03] #turn right + 3: [0.45, -0.06] #turn left hard + 4: [0.45, 0.06] #turn right hard + simple: + 0: [2, -1] + 1: [3, 0] + 2: [2, 1] + medium: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1, 1.5] + 4: [1, -1.5] + hard: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1.5, 1] + 4: [1.5, -1] + 5: [1, -1.5] + 6: [1, -1.5] + test: + 0: [0, 0] + +rewards: + follow_right_lane_only_center: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_step: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + +carla_environments: + follow_lane: + env_name: CarlaEnv-v0 + car: model1 + #sync_mode: True + weather: ClearNoon #dynamic, argparse + traffic_pedestrians: False + city_lights: False + car_lights: False + estimated_steps: 300 + save_episodes: 10 + save_every_step: 50 + filter: vehicle.* + generation: "2" + rolename: "hero" #name + gamma: 2.2 #for camera + sync: True #syncronous mode or async + waypoints_meters: 5 #distance between waypoints in meters + waypoints_init: 1 #Town07: (20, -1, 945) 951, #Town01: 879 crash testing, 839 init straight lane + waypoints_target: 1249 #Town07: (57, -1, 1249), #Town01: 959, 961 + waypoints_road_id: 8 # Town07: (20)-920-21-39-57, #Town01: 8 + max_target_waypoint_distance: 3 # distance to finish waypoint + waypoints_lane_id: lane-1 # lane-1 (right lane from south to north), lane1 + town: Town04_Opt #Town01, Town02, Town03, Town04, Town05, Town06, Town07, Town10HD, Town11 + # Town01_Opt, Town02_Opt, Town03_Opt, Town04_Opt, Town05_Opt, Town06_Opt, Town07_Opt, Town10HD_Opt + init_pose: + random_pose: False # random positions in the whole town + alternate_pose: True # random positions but only for selected poses in carla_town_positions->tonwXX->laneX->init_positions + init_pose_number: 0 + finish_pose_number: 0 + town_circuit: highway_circuit + +carla_town_positions: + Town04: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town04_Opt: + lateral_circuit: #up and down in lateral lanes + lane-1: + init_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #0: [32.3, 290.7, 0.0, 0.0, -44.1, 0.0] #x, y, z, pitch, yaw, roll + #1: [-399.6, 6.2, -0.0, -0.0, -176.0, 0.0] #x, y, z, pitch, yaw, roll + #2: [13.0, 310.3, -0.0, -0.0, -43.7, 0.0] #x, y, z, pitch, yaw, roll + #3: [-26.0, -250.6, -0.0, -0.0, 122.3, 0.0] #x, y, z, pitch, yaw, roll + #4: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #5: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + #6: [-380.9, -3.4, -0.0, 0.1, -93.2, 0.0] #x, y, z, pitch, yaw, roll + #7: [-15.9, -266.8, -0.0, -0.0, 121.3, 0.0] #x, y, z, pitch, yaw, roll + #8: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + finish_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + highway_circuit: + lane-1: + init_positions: + #0: [111.8, 38.1, 10.4, -1.1, -0.1, -0.2] #x, y, z, pitch, yaw, roll + 0: [-415.7, 6.0, -0.0, 0.0, -178.8, 0.0] #init + 1: [-21.2, 344.9, -0.0, -0.0, -45.1, 0.0] #x, y, z, pitch, yaw, roll + 2: [404.2, 24.4, 0.0, -0.0, -79.5, 0.0] #x, y, z, pitch, yaw, roll + 3: [-0.1, -295.6, -0.0, -0.0, 116.7, -0.0] #x, y, z, pitch, yaw, roll + 4: [410.947, -14.67, 0.2819, 0.0, -79.30, 0.0] + finish_positions: + #0: [-2.5, -290.8, -0.0, -0.0, 120.2, -0.0] #x, y, z, pitch, yaw, roll + 0: [-384.6, -8.5, -0.0, -0.0, 94.5, -0.0] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + + Town07_Opt: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] + #0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + + Town07: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + Town01_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town02_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town03_Opt: + town_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town05_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town06_Opt: + town_circuit: + lane-1: + init_positions: + 0: [102.1, 234.0, 0, 0, -14.9, 0] + 1: [479.2, 132.6, 0, -0, -24.3, -0] + 2: [87.7, 132.9, 0, 0, -11.1, 0] + finish_positions: + 0: [479.2, 132.6, 0, -0, -24.3, -0] + lane1: + init_positions: + finish_positions: + Town10HD_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town11: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: diff --git a/rl_studio/config/config_training_followlane_ddpg_carla.yaml b/rl_studio/config/config_training_followlane_ddpg_carla.yaml new file mode 100644 index 000000000..625d6fc16 --- /dev/null +++ b/rl_studio/config/config_training_followlane_ddpg_carla.yaml @@ -0,0 +1,365 @@ +settings: + mode: training # training, retraining, inference + task: follow_lane_carla # follow_line_gazebo, follow_lane_gazebo, autoparking_gazebo + algorithm: ddpg # qlearn, dqn, ddpg, ppo + simulator: carla # openai, carla, gazebo, sumo + environment_set: carla_environments # gazebo_environments, carla_environments + env: follow_lane # simple, nurburgring, montreal, curves, simple_laser, manual, autoparking + agent: auto_carla # f1, autoparkingRL, auto_carla, mountain_car, robot_mesh, cartpole, turtlebot + actions: continuous # continuous, simple, medium, hard, test, autoparking_simple + states: sp1 #sp4 #image, sp1 (simplified perception with 1 point), sp3 (simplified perception with 3 points), spn (simplified perception with n points) + rewards: rewards_sigmoid_only_right_line # follow_right_lane_only_center, follow_right_lane_center_v_step, follow_right_lane_center_v_w_linear + framework: TensorFlow # TensorFlow, Pytorch + total_episodes: 5000 + training_time: 16 + models_dir: "./checkpoints" + logs_dir: "./logs" + metrics_dir: "./metrics" + recorder_carla_dir: "./recorders" + +ros: + ros_master_uri: "11311" + gazebo_master_uri: "11345" + +carla: + carla_server: localhost + carla_client: 2000 + +retraining: + ddpg: + retrain_ddpg_tf_actor_model_name: "20230111_DDPG_Actor_conv2d32x64_Critic_conv2d32x64_BESTLAP_ACTOR_Max-69_Epoch-4_State-image_Actions-continuous_Rewards-follow_right_lane_center_v_w_linear_inTime-20230111-200026.h5" + retrain_ddpg_tf_critic_model_name: "20230111_DDPG_Actor_conv2d32x64_Critic_conv2d32x64_BESTLAP_CRITIC_Max-69_Epoch-4_State-image_Actions-continuous_Rewards-follow_right_lane_center_v_w_linear_inTime-20230111-200026.h5" + +inference: + ddpg: + inference_ddpg_tf_actor_model_name: + inference_ddpg_tf_critic_model_name: + +algorithm: + ddpg: + gamma: 0.99 + tau: 0.005 + std_dev: 0.2 + model_name: DDPG_Actor_conv2d32x64_Critic_conv2d32x64 + replay_memory_size: 50_000 + memory_fraction: 0.20 + critic_lr: 0.002 + actor_lr: 0.001 + buffer_capacity: 100_000 + batch_size: 64 + +agents: + auto_carla: + camera_params: + width: 640 + height: 480 + center_image: 320 + raw_image: False + image_resizing: 100 + new_image_size: 32 + num_regions: 16 + lower_limit: 220 + +states: + image: + 0: [30, 40, 50, 60] # try up to 4 points + sp1: + 0: [50] + sp3: + 0: [5, 15, 22] + sp4: + 0: [30, 40, 50, 60] #[50, 70, 90, 120] #[30, 50, 70, 90] #[90, 130, 170, 210] #[30, 60, 100, 120] + sp5: + 0: [3, 5, 10, 15, 20] + spn: + 0: [10] + states_entry: only_right_line # only_right_line, between_right_left_lines, lane_detector_pytorch + +actions: + continuous: + v: [0.4, 0.4] #throttle + w: [-0.07000, 0.07000] #steering + simple: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + medium: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1, 1.5] + 4: [1, -1.5] + hard: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1.5, 1] + 4: [1.5, -1] + 5: [1, -1.5] + 6: [1, -1.5] + test: + 0: [0, 0] + +rewards: + rewards_sigmoid_only_right_line: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_only_center: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_step: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_w_linear: # only for continuous actions + beta_0: 3 + beta_1: -0.1 + penal: 0 + min_reward: 1_000 + highest_reward: 100 + +carla_environments: + follow_lane: + env_name: CarlaEnv-v0 + car: model1 + #sync_mode: True + weather: ClearNoon #dynamic, argparse + traffic_pedestrians: False + city_lights: False + car_lights: False + estimated_steps: 300 + save_episodes: 5 + save_every_step: 50 + filter: vehicle.* + generation: "2" + rolename: "hero" #name + gamma: 2.2 #for camera + sync: True #syncronous mode or async + waypoints_meters: 5 #distance between waypoints in meters + waypoints_init: 1 #Town07: (20, -1, 945) 951, #Town01: 879 crash testing, 839 init straight lane + waypoints_target: 1249 #Town07: (57, -1, 1249), #Town01: 959, 961 + waypoints_road_id: 8 # Town07: (20)-920-21-39-57, #Town01: 8 + max_target_waypoint_distance: 3 # distance to finish waypoint + waypoints_lane_id: lane-1 # lane-1 (right lane from south to north), lane1 + town: Town04_Opt #Town01, Town02, Town03, Town04, Town05, Town06, Town07, Town10HD, Town11 + # Town01_Opt, Town02_Opt, Town03_Opt, Town04_Opt, Town05_Opt, Town06_Opt, Town07_Opt, Town10HD_Opt + init_pose: + random_pose: False # random positions in the whole town + alternate_pose: False # random positions but only for selected poses in carla_town_positions->tonwXX->laneX->init_positions + init_pose_number: 0 + finish_pose_number: 0 + town_circuit: highway_circuit + +carla_town_positions: + Town04: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town04_Opt: + lateral_circuit: #up and down in lateral lanes + lane-1: + init_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #0: [32.3, 290.7, 0.0, 0.0, -44.1, 0.0] #x, y, z, pitch, yaw, roll + #1: [-399.6, 6.2, -0.0, -0.0, -176.0, 0.0] #x, y, z, pitch, yaw, roll + #2: [13.0, 310.3, -0.0, -0.0, -43.7, 0.0] #x, y, z, pitch, yaw, roll + #3: [-26.0, -250.6, -0.0, -0.0, 122.3, 0.0] #x, y, z, pitch, yaw, roll + #4: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #5: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + #6: [-380.9, -3.4, -0.0, 0.1, -93.2, 0.0] #x, y, z, pitch, yaw, roll + #7: [-15.9, -266.8, -0.0, -0.0, 121.3, 0.0] #x, y, z, pitch, yaw, roll + #8: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + finish_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + highway_circuit: + lane-1: + init_positions: + #0: [111.8, 38.1, 10.4, -1.1, -0.1, -0.2] #x, y, z, pitch, yaw, roll + 0: [-415.7, 6.0, -0.0, 0.0, -178.8, 0.0] #init + 1: [-21.2, 344.9, -0.0, -0.0, -45.1, 0.0] #x, y, z, pitch, yaw, roll + 2: [404.2, 24.4, 0.0, -0.0, -79.5, 0.0] #x, y, z, pitch, yaw, roll + 3: [-0.1, -295.6, -0.0, -0.0, 116.7, -0.0] #x, y, z, pitch, yaw, roll + 4: [410.947, -14.67, 0.2819, 0.0, -79.30, 0.0] + finish_positions: + #0: [-2.5, -290.8, -0.0, -0.0, 120.2, -0.0] #x, y, z, pitch, yaw, roll + 0: [-384.6, -8.5, -0.0, -0.0, 94.5, -0.0] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + + Town07_Opt: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] + #0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + + Town07: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + Town01_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town02_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town03_Opt: + town_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town05_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town06_Opt: + town_circuit: + lane-1: + init_positions: + 0: [102.1, 234.0, 0, 0, -14.9, 0] + 1: [479.2, 132.6, 0, -0, -24.3, -0] + 2: [87.7, 132.9, 0, 0, -11.1, 0] + finish_positions: + 0: [479.2, 132.6, 0, -0, -24.3, -0] + lane1: + init_positions: + finish_positions: + Town10HD_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town11: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: diff --git a/rl_studio/config/config_training_followlane_ddpg_f1_gazebo.yaml b/rl_studio/config/config_training_followlane_ddpg_f1_gazebo.yaml index 58310cdda..95edd4099 100644 --- a/rl_studio/config/config_training_followlane_ddpg_f1_gazebo.yaml +++ b/rl_studio/config/config_training_followlane_ddpg_f1_gazebo.yaml @@ -34,14 +34,14 @@ algorithm: ddpg: gamma: 0.9 tau: 0.005 - std_dev: 0.2 + std_dev: 0.01 model_name: DDPG_Actor_conv2d32x64_Critic_conv2d32x64 replay_memory_size: 50_000 memory_fraction: 0.20 critic_lr: 0.002 actor_lr: 0.001 buffer_capacity: 100_000 - batch_size: 64 + batch_size: 32 agents: f1: diff --git a/rl_studio/config/config_training_followlane_dqn_carla.yaml b/rl_studio/config/config_training_followlane_dqn_carla.yaml new file mode 100644 index 000000000..a75af9811 --- /dev/null +++ b/rl_studio/config/config_training_followlane_dqn_carla.yaml @@ -0,0 +1,473 @@ +settings: + station: my_local_PC # landau + mode: training # training, retraining, inference + task: follow_lane_carla # follow_line_gazebo, follow_lane_gazebo, autoparking_gazebo + algorithm: dqn # qlearn, dqn, ddpg, ppo + simulator: carla # openai, carla, gazebo, sumo + environment_set: carla_environments # gazebo_environments, carla_environments + env: follow_lane # simple, nurburgring, montreal, curves, simple_laser, manual, autoparking + agent: auto_carla # f1, autoparkingRL, auto_carla, mountain_car, robot_mesh, cartpole, turtlebot + actions: carla_discrete # continuous, simple, medium, hard, test, autoparking_simple, carla_discrete + states: image #sp4 #image, sp1 (simplified perception with 1 point), sp3 (simplified perception with 3 points), spn (simplified perception with n points) + rewards: rewards_followlane_center_velocity_angle # follow_right_lane_only_center, follow_right_lane_center_v_step, follow_right_lane_center_v_w_linear + framework: TensorFlow # TensorFlow, Pytorch + total_episodes: 500 + training_time: 16 + models_dir: "./checkpoints" + logs_dir: "./logs" + metrics_dir: "./metrics" + recorder_carla_dir: "./recorders" + +ros: + ros_master_uri: "11311" + gazebo_master_uri: "11345" + +carla: + carla_server: localhost + carla_client: 2000 + traffic_manager_port: 4637 + +retraining: + dqn: + retrain_dqn_tf_model_name: "20230131-212216_Circuit-simple_States-sp1_Actions-simple_BESTLAP_Rewards-follow_right_lane_only_center_epsilon-0.459_epoch-2585_step-943_reward-4700_DQN_sp_16x16.h5" + ddpg: + retrain_ddpg_tf_actor_model_name: "20230111_DDPG_Actor_conv2d32x64_Critic_conv2d32x64_BESTLAP_ACTOR_Max-69_Epoch-4_State-image_Actions-continuous_Rewards-follow_right_lane_center_v_w_linear_inTime-20230111-200026.h5" + retrain_ddpg_tf_critic_model_name: "20230111_DDPG_Actor_conv2d32x64_Critic_conv2d32x64_BESTLAP_CRITIC_Max-69_Epoch-4_State-image_Actions-continuous_Rewards-follow_right_lane_center_v_w_linear_inTime-20230111-200026.h5" + +inference: + ddpg: + inference_ddpg_tf_actor_model_name: + inference_ddpg_tf_critic_model_name: + dqn: + inference_dqn_tf_model_name: + +algorithm: + dqn: + #alpha: 0.95 + gamma: 0.99 # future values in the long term reward. When close to 1, we reward long term + epsilon: 0.99 # in Retraining mode is convenient to reduce, i.e. 0.45 + epsilon_discount: 0.9986 + epsilon_min: 0.00 + model_name: SEQ_32x32 + replay_memory_size: 10_000 # How many last steps to keep for model training + min_replay_memory_size: 1000 #1000 # Minimum number of steps in a memory to start training + minibatch_size: 64 # How many steps (samples) to use for training + update_target_every: 5 + memory_fraction: 0.20 + #buffer_capacity: 100_000 + #batch_size: 64 + ddpg: + gamma: 0.99 + tau: 0.005 + std_dev: 0.2 + model_name: DDPG_Actor_conv2d32x64_Critic_conv2d32x64 + replay_memory_size: 50_000 + memory_fraction: 0.20 + critic_lr: 0.002 + actor_lr: 0.001 + buffer_capacity: 100_000 + batch_size: 64 + +agents: + auto_carla: + targets: + target_vel: 10 + camera_params: + width: 640 + height: 480 + center_image: 320 + raw_image: False + image_resizing: 100 + new_image_size: 32 + num_regions: 23 # this should be named states!!! + lower_limit: 220 + +states: # should be named as points of percepction (states name is wrong) + image_and_vector: + 0: [30, 40, 50, 60, 70, 80, 90] + image: + 0: [50, 60, 70, 80] # try up to 4 points + sp1: + 0: [50] + sp3: + 0: [5, 15, 22] + sp4: + 0: [50, 60, 70, 80] #[50, 70, 90, 120] #[30, 50, 70, 90] #[90, 130, 170, 210] #[30, 60, 100, 120] + sp7: + 0: [30, 40, 50, 60, 70, 80, 90] #[30, 35, 40, 45, 50, 55, 60] + spn: + 0: [10] + states_entry: lane_detector_pytorch # only_right_line, between_right_left_lines, lane_detector_pytorch + +actions: + carla_discrete: + 0: [0.55, 0.0] #straight forward + 1: [0.3, 0.1] + 2: [0.3, 0.15] + 3: [0.3, -0.1] + 4: [0.3, -0.15] + 5: [0.35, 0.07] + 6: [0.35, 0.09] + 7: [0.35, -0.07] + 8: [0.35, -0.09] + 9: [0.4, 0.03] + 10: [0.4, 0.05] + 11: [0.4, 0.07] + 12: [0.4, -0.03] + 13: [0.4, -0.05] + 14: [0.4, -0.07] + 15: [0.45, 0.03] + 16: [0.45, 0.05] + 17: [0.45, -0.03] + 18: [0.45, -0.05] + 19: [0.50, 0.01] + 20: [0.5, 0.03] + 21: [0.5, -0.01] + 22: [0.5, -0.03] + #0: [0.5, 0.0] #straight forward + #1: [0.2, 0.1] + #2: [0.2, 0.2] + #3: [0.2, -0.1] + #4: [0.2, -0.2] + #5: [0.25, 0.07] + #6: [0.25, 0.1] + #7: [0.25, -0.07] + #8: [0.25, -0.1] + #9: [0.3, 0.03] + #10: [0.3, 0.05] + #11: [0.3, 0.07] + #12: [0.3, -0.03] + #13: [0.3, -0.05] + #14: [0.3, -0.7] + #15: [0.35, 0.03] + #16: [0.35, 0.05] + #17: [0.35, -0.03] + #18: [0.35, -0.05] + #19: [0.45, 0.01] + #20: [0.45, 0.03] + #21: [0.45, -0.01] + #22: [0.45, -0.03] + #0: [0.6, 0.0] #straight forward + #1: [0.2, 0.1] + #2: [0.2, 0.2] + #3: [0.2, -0.1] + #4: [0.2, -0.2] + #5: [0.3, 0.07] + #6: [0.3, 0.1] + #7: [0.3, -0.07] + #8: [0.3, -0.1] + #9: [0.4, 0.03] + #10: [0.4, 0.05] + #11: [0.4, 0.07] + #12: [0.4, -0.03] + #13: [0.4, -0.05] + #14: [0.4, -0.7] + #15: [0.5, 0.03] + #16: [0.5, 0.05] + #17: [0.5, -0.03] + #18: [0.5, -0.05] + #19: [0.55, 0.01] + #20: [0.55, 0.03] + #21: [0.55, -0.01] + #22: [0.55, -0.03] + #0: [0.5, -0.03] #turn left + #1: [0.55, 0.0] #straight forward + #2: [0.5, 0.03] #turn right + #3: [0.45, -0.07] #turn left hard + #4: [0.45, 0.07] #turn right hard + #5: [0.4, -0.1] #turn left + #6: [0.4, 0.1] #straight forward + continuous: + v: [0.4, 0.4] #throttle + w: [-0.07000, 0.07000] #steering + simple: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + medium: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1, 1.5] + 4: [1, -1.5] + hard: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1.5, 1] + 4: [1.5, -1] + 5: [1, -1.5] + 6: [1, -1.5] + test: + 0: [0, 0] + +rewards: + rewards_followlane_center_velocity_angle: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + rewards_sigmoid_only_right_line: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_only_center: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_step: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_w_linear: # only for continuous actions + beta_0: 3 + beta_1: -0.1 + penal: 0 + min_reward: 1_000 + highest_reward: 100 + +carla_environments: + follow_lane: + env_name: CarlaEnv-v0 + car: model1 + #sync_mode: True + weather: ClearNoon #dynamic, argparse + traffic_pedestrians: False + city_lights: False + car_lights: False + estimated_steps: 500 + save_episodes: 20 + save_every_step: 50 + filter: vehicle.* + generation: "2" + rolename: "hero" #name + gamma: 2.2 #for camera + sync: True #syncronous mode or async + waypoints_meters: 5 #distance between waypoints in meters + waypoints_init: 1 #Town07: (20, -1, 945) 951, #Town01: 879 crash testing, 839 init straight lane + waypoints_target: 1249 #Town07: (57, -1, 1249), #Town01: 959, 961 + waypoints_road_id: 8 # Town07: (20)-920-21-39-57, #Town01: 8 + max_target_waypoint_distance: 3 # distance to finish waypoint + waypoints_lane_id: lane-1 # lane-1 (right lane from south to north), lane1 + town: Town04_Opt #Town01, Town02, Town03, Town04, Town05, Town06, Town07, Town10HD, Town11 + # Town01_Opt, Town02_Opt, Town03_Opt, Town04_Opt, Town05_Opt, Town06_Opt, Town07_Opt, Town10HD_Opt + init_pose: + random_pose: False # random positions in the whole town + alternate_pose: True #random positions but only for selected poses in carla_town_positions->tonwXX->laneX->init_positions + init_pose_number: 0 + finish_pose_number: 0 + town_circuit: highway_circuit + +carla_town_positions: + Town04: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town04_Opt: + lateral_circuit: #up and down in lateral lanes + lane-1: + init_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #0: [32.3, 290.7, 0.0, 0.0, -44.1, 0.0] #x, y, z, pitch, yaw, roll + #1: [-399.6, 6.2, -0.0, -0.0, -176.0, 0.0] #x, y, z, pitch, yaw, roll + #2: [13.0, 310.3, -0.0, -0.0, -43.7, 0.0] #x, y, z, pitch, yaw, roll + #3: [-26.0, -250.6, -0.0, -0.0, 122.3, 0.0] #x, y, z, pitch, yaw, roll + #4: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #5: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + #6: [-380.9, -3.4, -0.0, 0.1, -93.2, 0.0] #x, y, z, pitch, yaw, roll + #7: [-15.9, -266.8, -0.0, -0.0, 121.3, 0.0] #x, y, z, pitch, yaw, roll + #8: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + finish_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + highway_circuit: + lane-1: + init_positions: + #0: [111.8, 38.1, 10.4, -1.1, -0.1, -0.2] #x, y, z, pitch, yaw, roll + 0: [-415.7, 6.0, -0.0, 0.0, -178.8, 0.0] #init + 1: [-21.2, 344.9, -0.0, -0.0, -45.1, 0.0] #x, y, z, pitch, yaw, roll + 2: [404.2, 24.4, 0.0, -0.0, -79.5, 0.0] #x, y, z, pitch, yaw, roll + 3: [-0.1, -295.6, -0.0, -0.0, 116.7, -0.0] #x, y, z, pitch, yaw, roll + 4: [410.947, -14.67, 0.2819, 0.0, -79.30, 0.0] + finish_positions: + #0: [-2.5, -290.8, -0.0, -0.0, 120.2, -0.0] #x, y, z, pitch, yaw, roll + 0: [-384.6, -8.5, -0.0, -0.0, 94.5, -0.0] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + + Town07_Opt: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] + #0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + + Town07: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + Town01_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town02_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town03_Opt: + town_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town05_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town06_Opt: + town_circuit: + lane-1: + init_positions: + 0: [102.1, 234.0, 0, 0, -14.9, 0] + 1: [479.2, 132.6, 0, -0, -24.3, -0] + 2: [87.7, 132.9, 0, 0, -11.1, 0] + finish_positions: + 0: [479.2, 132.6, 0, -0, -24.3, -0] + lane1: + init_positions: + finish_positions: + Town10HD_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town11: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: diff --git a/rl_studio/config/config_training_followlane_dqn_carla_landau.yaml b/rl_studio/config/config_training_followlane_dqn_carla_landau.yaml new file mode 100644 index 000000000..774f66219 --- /dev/null +++ b/rl_studio/config/config_training_followlane_dqn_carla_landau.yaml @@ -0,0 +1,471 @@ +settings: + station: landau # landau + mode: training # training, retraining, inference + task: follow_lane_carla # follow_line_gazebo, follow_lane_gazebo, autoparking_gazebo + algorithm: dqn # qlearn, dqn, ddpg, ppo + simulator: carla # openai, carla, gazebo, sumo + environment_set: carla_environments # gazebo_environments, carla_environments + env: follow_lane # simple, nurburgring, montreal, curves, simple_laser, manual, autoparking + agent: auto_carla # f1, autoparkingRL, auto_carla, mountain_car, robot_mesh, cartpole, turtlebot + actions: carla_discrete # continuous, simple, medium, hard, test, autoparking_simple, carla_discrete + states: sp7 #sp4 #image, sp1 (simplified perception with 1 point), sp3 (simplified perception with 3 points), spn (simplified perception with n points) + rewards: rewards_followlane_center_velocity_angle # follow_right_lane_only_center, follow_right_lane_center_v_step, follow_right_lane_center_v_w_linear + framework: TensorFlow # TensorFlow, Pytorch + total_episodes: 10_000 + training_time: 200 + models_dir: "./checkpoints" + logs_dir: "./logs" + metrics_dir: "./metrics" + recorder_carla_dir: "./recorders" + +ros: + ros_master_uri: "11311" + gazebo_master_uri: "11345" + +carla: + carla_server: localhost + carla_client: 4356 + traffic_manager_port: 4637 + +retraining: + dqn: + retrain_dqn_tf_model_name: "20230131-212216_Circuit-simple_States-sp1_Actions-simple_BESTLAP_Rewards-follow_right_lane_only_center_epsilon-0.459_epoch-2585_step-943_reward-4700_DQN_sp_16x16.h5" + ddpg: + retrain_ddpg_tf_actor_model_name: "20230111_DDPG_Actor_conv2d32x64_Critic_conv2d32x64_BESTLAP_ACTOR_Max-69_Epoch-4_State-image_Actions-continuous_Rewards-follow_right_lane_center_v_w_linear_inTime-20230111-200026.h5" + retrain_ddpg_tf_critic_model_name: "20230111_DDPG_Actor_conv2d32x64_Critic_conv2d32x64_BESTLAP_CRITIC_Max-69_Epoch-4_State-image_Actions-continuous_Rewards-follow_right_lane_center_v_w_linear_inTime-20230111-200026.h5" + +inference: + ddpg: + inference_ddpg_tf_actor_model_name: + inference_ddpg_tf_critic_model_name: + dqn: + inference_dqn_tf_model_name: + +algorithm: + dqn: + #alpha: 0.95 + gamma: 0.99 # future values in the long term reward. When close to 1, we reward long term + epsilon: 0.99 # in Retraining mode is convenient to reduce, i.e. 0.45 + epsilon_discount: 0.9986 + epsilon_min: 0.00 + model_name: SEQ_32x32 + replay_memory_size: 10_000 # How many last steps to keep for model training + min_replay_memory_size: 1000 #1000 # Minimum number of steps in a memory to start training + minibatch_size: 64 # How many steps (samples) to use for training + update_target_every: 5 + memory_fraction: 0.20 + #buffer_capacity: 100_000 + #batch_size: 64 + ddpg: + gamma: 0.99 + tau: 0.005 + std_dev: 0.2 + model_name: DDPG_Actor_conv2d32x64_Critic_conv2d32x64 + replay_memory_size: 50_000 + memory_fraction: 0.20 + critic_lr: 0.002 + actor_lr: 0.001 + buffer_capacity: 100_000 + batch_size: 64 + +agents: + auto_carla: + targets: + target_vel: 10 + camera_params: + width: 640 + height: 480 + center_image: 320 + raw_image: False + image_resizing: 100 + new_image_size: 32 + num_regions: 23 # this should be named states!!! + lower_limit: 220 + +states: # should be named as points of percepction (states name is wrong) + image: + 0: [50, 60, 70, 80] # try up to 4 points + sp1: + 0: [50] + sp3: + 0: [5, 15, 22] + sp4: + 0: [50, 60, 70, 80] #[50, 70, 90, 120] #[30, 50, 70, 90] #[90, 130, 170, 210] #[30, 60, 100, 120] + sp7: + 0: [30, 40, 50, 60, 70, 80, 90] #[30, 35, 40, 45, 50, 55, 60] + spn: + 0: [10] + states_entry: lane_detector_pytorch # only_right_line, between_right_left_lines, lane_detector_pytorch + +actions: + carla_discrete: + 0: [0.55, 0.0] #straight forward + 1: [0.3, 0.1] + 2: [0.3, 0.15] + 3: [0.3, -0.1] + 4: [0.3, -0.15] + 5: [0.35, 0.07] + 6: [0.35, 0.09] + 7: [0.35, -0.07] + 8: [0.35, -0.09] + 9: [0.4, 0.03] + 10: [0.4, 0.05] + 11: [0.4, 0.07] + 12: [0.4, -0.03] + 13: [0.4, -0.05] + 14: [0.4, -0.07] + 15: [0.45, 0.03] + 16: [0.45, 0.05] + 17: [0.45, -0.03] + 18: [0.45, -0.05] + 19: [0.50, 0.01] + 20: [0.5, 0.03] + 21: [0.5, -0.01] + 22: [0.5, -0.03] + #0: [0.5, 0.0] #straight forward + #1: [0.2, 0.1] + #2: [0.2, 0.2] + #3: [0.2, -0.1] + #4: [0.2, -0.2] + #5: [0.25, 0.07] + #6: [0.25, 0.1] + #7: [0.25, -0.07] + #8: [0.25, -0.1] + #9: [0.3, 0.03] + #10: [0.3, 0.05] + #11: [0.3, 0.07] + #12: [0.3, -0.03] + #13: [0.3, -0.05] + #14: [0.3, -0.7] + #15: [0.35, 0.03] + #16: [0.35, 0.05] + #17: [0.35, -0.03] + #18: [0.35, -0.05] + #19: [0.45, 0.01] + #20: [0.45, 0.03] + #21: [0.45, -0.01] + #22: [0.45, -0.03] + #0: [0.6, 0.0] #straight forward + #1: [0.2, 0.1] + #2: [0.2, 0.2] + #3: [0.2, -0.1] + #4: [0.2, -0.2] + #5: [0.3, 0.07] + #6: [0.3, 0.1] + #7: [0.3, -0.07] + #8: [0.3, -0.1] + #9: [0.4, 0.03] + #10: [0.4, 0.05] + #11: [0.4, 0.07] + #12: [0.4, -0.03] + #13: [0.4, -0.05] + #14: [0.4, -0.7] + #15: [0.5, 0.03] + #16: [0.5, 0.05] + #17: [0.5, -0.03] + #18: [0.5, -0.05] + #19: [0.55, 0.01] + #20: [0.55, 0.03] + #21: [0.55, -0.01] + #22: [0.55, -0.03] + #0: [0.5, -0.03] #turn left + #1: [0.55, 0.0] #straight forward + #2: [0.5, 0.03] #turn right + #3: [0.45, -0.07] #turn left hard + #4: [0.45, 0.07] #turn right hard + #5: [0.4, -0.1] #turn left + #6: [0.4, 0.1] #straight forward + continuous: + v: [0.4, 0.4] #throttle + w: [-0.07000, 0.07000] #steering + simple: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + medium: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1, 1.5] + 4: [1, -1.5] + hard: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1.5, 1] + 4: [1.5, -1] + 5: [1, -1.5] + 6: [1, -1.5] + test: + 0: [0, 0] + +rewards: + rewards_followlane_center_velocity_angle: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + rewards_sigmoid_only_right_line: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_only_center: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_step: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_w_linear: # only for continuous actions + beta_0: 3 + beta_1: -0.1 + penal: 0 + min_reward: 1_000 + highest_reward: 100 + +carla_environments: + follow_lane: + env_name: CarlaEnv-v0 + car: model1 + #sync_mode: True + weather: ClearNoon #dynamic, argparse + traffic_pedestrians: False + city_lights: False + car_lights: False + estimated_steps: 700 + save_episodes: 10 + save_every_step: 50 + filter: vehicle.* + generation: "2" + rolename: "hero" #name + gamma: 2.2 #for camera + sync: True #syncronous mode or async + waypoints_meters: 5 #distance between waypoints in meters + waypoints_init: 1 #Town07: (20, -1, 945) 951, #Town01: 879 crash testing, 839 init straight lane + waypoints_target: 1249 #Town07: (57, -1, 1249), #Town01: 959, 961 + waypoints_road_id: 8 # Town07: (20)-920-21-39-57, #Town01: 8 + max_target_waypoint_distance: 3 # distance to finish waypoint + waypoints_lane_id: lane-1 # lane-1 (right lane from south to north), lane1 + town: Town04_Opt #Town01, Town02, Town03, Town04, Town05, Town06, Town07, Town10HD, Town11 + # Town01_Opt, Town02_Opt, Town03_Opt, Town04_Opt, Town05_Opt, Town06_Opt, Town07_Opt, Town10HD_Opt + init_pose: + random_pose: False # random positions in the whole town + alternate_pose: True #random positions but only for selected poses in carla_town_positions->tonwXX->laneX->init_positions + init_pose_number: 0 + finish_pose_number: 0 + town_circuit: highway_circuit + +carla_town_positions: + Town04: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town04_Opt: + lateral_circuit: #up and down in lateral lanes + lane-1: + init_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #0: [32.3, 290.7, 0.0, 0.0, -44.1, 0.0] #x, y, z, pitch, yaw, roll + #1: [-399.6, 6.2, -0.0, -0.0, -176.0, 0.0] #x, y, z, pitch, yaw, roll + #2: [13.0, 310.3, -0.0, -0.0, -43.7, 0.0] #x, y, z, pitch, yaw, roll + #3: [-26.0, -250.6, -0.0, -0.0, 122.3, 0.0] #x, y, z, pitch, yaw, roll + #4: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #5: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + #6: [-380.9, -3.4, -0.0, 0.1, -93.2, 0.0] #x, y, z, pitch, yaw, roll + #7: [-15.9, -266.8, -0.0, -0.0, 121.3, 0.0] #x, y, z, pitch, yaw, roll + #8: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + finish_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + highway_circuit: + lane-1: + init_positions: + #0: [111.8, 38.1, 10.4, -1.1, -0.1, -0.2] #x, y, z, pitch, yaw, roll + 0: [-415.7, 6.0, -0.0, 0.0, -178.8, 0.0] #init + 1: [-21.2, 344.9, -0.0, -0.0, -45.1, 0.0] #x, y, z, pitch, yaw, roll + 2: [404.2, 24.4, 0.0, -0.0, -79.5, 0.0] #x, y, z, pitch, yaw, roll + 3: [-0.1, -295.6, -0.0, -0.0, 116.7, -0.0] #x, y, z, pitch, yaw, roll + 4: [410.947, -14.67, 0.2819, 0.0, -79.30, 0.0] + finish_positions: + #0: [-2.5, -290.8, -0.0, -0.0, 120.2, -0.0] #x, y, z, pitch, yaw, roll + 0: [-384.6, -8.5, -0.0, -0.0, 94.5, -0.0] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + + Town07_Opt: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] + #0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + + Town07: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + Town01_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town02_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town03_Opt: + town_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town05_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town06_Opt: + town_circuit: + lane-1: + init_positions: + 0: [102.1, 234.0, 0, 0, -14.9, 0] + 1: [479.2, 132.6, 0, -0, -24.3, -0] + 2: [87.7, 132.9, 0, 0, -11.1, 0] + finish_positions: + 0: [479.2, 132.6, 0, -0, -24.3, -0] + lane1: + init_positions: + finish_positions: + Town10HD_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town11: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: diff --git a/rl_studio/config/config_training_followlane_dqn_carla_landau_lane1.yaml b/rl_studio/config/config_training_followlane_dqn_carla_landau_lane1.yaml new file mode 100644 index 000000000..4e890808f --- /dev/null +++ b/rl_studio/config/config_training_followlane_dqn_carla_landau_lane1.yaml @@ -0,0 +1,472 @@ +settings: + station: landau # landau + mode: training # training, retraining, inference + task: follow_lane_carla # follow_line_gazebo, follow_lane_gazebo, autoparking_gazebo + algorithm: dqn # qlearn, dqn, ddpg, ppo + simulator: carla # openai, carla, gazebo, sumo + environment_set: carla_environments # gazebo_environments, carla_environments + env: follow_lane # simple, nurburgring, montreal, curves, simple_laser, manual, autoparking + agent: auto_carla # f1, autoparkingRL, auto_carla, mountain_car, robot_mesh, cartpole, turtlebot + actions: carla_discrete # continuous, simple, medium, hard, test, autoparking_simple, carla_discrete + states: sp7 #sp4 #image, sp1 (simplified perception with 1 point), sp3 (simplified perception with 3 points), spn (simplified perception with n points) + rewards: rewards_followlane_center_velocity_angle # follow_right_lane_only_center, follow_right_lane_center_v_step, follow_right_lane_center_v_w_linear + framework: TensorFlow # TensorFlow, Pytorch + total_episodes: 20_000 + training_time: 16 + models_dir: "./checkpoints" + logs_dir: "./logs" + metrics_dir: "./metrics" + recorder_carla_dir: "./recorders" + +ros: + ros_master_uri: "11311" + gazebo_master_uri: "11345" + +carla: + carla_server: localhost + carla_client: 4356 + traffic_manager_port: 4637 + +retraining: + dqn: + retrain_dqn_tf_model_name: "20230131-212216_Circuit-simple_States-sp1_Actions-simple_BESTLAP_Rewards-follow_right_lane_only_center_epsilon-0.459_epoch-2585_step-943_reward-4700_DQN_sp_16x16.h5" + ddpg: + retrain_ddpg_tf_actor_model_name: "20230111_DDPG_Actor_conv2d32x64_Critic_conv2d32x64_BESTLAP_ACTOR_Max-69_Epoch-4_State-image_Actions-continuous_Rewards-follow_right_lane_center_v_w_linear_inTime-20230111-200026.h5" + retrain_ddpg_tf_critic_model_name: "20230111_DDPG_Actor_conv2d32x64_Critic_conv2d32x64_BESTLAP_CRITIC_Max-69_Epoch-4_State-image_Actions-continuous_Rewards-follow_right_lane_center_v_w_linear_inTime-20230111-200026.h5" + +inference: + ddpg: + inference_ddpg_tf_actor_model_name: + inference_ddpg_tf_critic_model_name: + dqn: + inference_dqn_tf_model_name: + +algorithm: + dqn: + #alpha: 0.95 + gamma: 0.99 # future values in the long term reward. When close to 1, we reward long term + epsilon: 0.99 # in Retraining mode is convenient to reduce, i.e. 0.45 + epsilon_discount: 0.9986 + epsilon_min: 0.00 + model_name: SEQ_32x32 + replay_memory_size: 10_000 # How many last steps to keep for model training + min_replay_memory_size: 1000 #1000 # Minimum number of steps in a memory to start training + minibatch_size: 64 # How many steps (samples) to use for training + update_target_every: 5 + memory_fraction: 0.20 + #buffer_capacity: 100_000 + #batch_size: 64 + ddpg: + gamma: 0.99 + tau: 0.005 + std_dev: 0.2 + model_name: DDPG_Actor_conv2d32x64_Critic_conv2d32x64 + replay_memory_size: 50_000 + memory_fraction: 0.20 + critic_lr: 0.002 + actor_lr: 0.001 + buffer_capacity: 100_000 + batch_size: 64 + +agents: + auto_carla: + targets: + target_vel: 10 + camera_params: + width: 640 + height: 480 + center_image: 320 + raw_image: False + image_resizing: 100 + new_image_size: 32 + num_regions: 23 # this should be named states!!! + lower_limit: 220 + +states: # should be named as points of percepction (states name is wrong) + image: + 0: [50, 60, 70, 80] # try up to 4 points + sp1: + 0: [50] + sp3: + 0: [5, 15, 22] + sp4: + 0: [50, 60, 70, 80] #[50, 70, 90, 120] #[30, 50, 70, 90] #[90, 130, 170, 210] #[30, 60, 100, 120] + sp7: + 0: [30, 40, 50, 60, 70, 80, 90] #[30, 35, 40, 45, 50, 55, 60] + spn: + 0: [10] + states_entry: lane_detector_pytorch # only_right_line, between_right_left_lines, lane_detector_pytorch + +actions: + carla_discrete: + 0: [0.55, 0.0] #straight forward + 1: [0.3, 0.1] + 2: [0.3, 0.15] + 3: [0.3, -0.1] + 4: [0.3, -0.15] + 5: [0.35, 0.07] + 6: [0.35, 0.09] + 7: [0.35, -0.07] + 8: [0.35, -0.09] + 9: [0.4, 0.03] + 10: [0.4, 0.05] + 11: [0.4, 0.07] + 12: [0.4, -0.03] + 13: [0.4, -0.05] + 14: [0.4, -0.07] + 15: [0.45, 0.03] + 16: [0.45, 0.05] + 17: [0.45, -0.03] + 18: [0.45, -0.05] + 19: [0.50, 0.01] + 20: [0.5, 0.03] + 21: [0.5, -0.01] + 22: [0.5, -0.03] + #0: [0.5, 0.0] #straight forward + #1: [0.2, 0.1] + #2: [0.2, 0.2] + #3: [0.2, -0.1] + #4: [0.2, -0.2] + #5: [0.25, 0.07] + #6: [0.25, 0.1] + #7: [0.25, -0.07] + #8: [0.25, -0.1] + #9: [0.3, 0.03] + #10: [0.3, 0.05] + #11: [0.3, 0.07] + #12: [0.3, -0.03] + #13: [0.3, -0.05] + #14: [0.3, -0.7] + #15: [0.35, 0.03] + #16: [0.35, 0.05] + #17: [0.35, -0.03] + #18: [0.35, -0.05] + #19: [0.45, 0.01] + #20: [0.45, 0.03] + #21: [0.45, -0.01] + #22: [0.45, -0.03] + #0: [0.6, 0.0] #straight forward + #1: [0.2, 0.1] + #2: [0.2, 0.2] + #3: [0.2, -0.1] + #4: [0.2, -0.2] + #5: [0.3, 0.07] + #6: [0.3, 0.1] + #7: [0.3, -0.07] + #8: [0.3, -0.1] + #9: [0.4, 0.03] + #10: [0.4, 0.05] + #11: [0.4, 0.07] + #12: [0.4, -0.03] + #13: [0.4, -0.05] + #14: [0.4, -0.7] + #15: [0.5, 0.03] + #16: [0.5, 0.05] + #17: [0.5, -0.03] + #18: [0.5, -0.05] + #19: [0.55, 0.01] + #20: [0.55, 0.03] + #21: [0.55, -0.01] + #22: [0.55, -0.03] + #0: [0.5, -0.03] #turn left + #1: [0.55, 0.0] #straight forward + #2: [0.5, 0.03] #turn right + #3: [0.45, -0.07] #turn left hard + #4: [0.45, 0.07] #turn right hard + #5: [0.4, -0.1] #turn left + #6: [0.4, 0.1] #straight forward + continuous: + v: [0.4, 0.4] #throttle + w: [-0.07000, 0.07000] #steering + simple: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + medium: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1, 1.5] + 4: [1, -1.5] + hard: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1.5, 1] + 4: [1.5, -1] + 5: [1, -1.5] + 6: [1, -1.5] + test: + 0: [0, 0] + +rewards: + rewards_followlane_center_velocity_angle: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + rewards_sigmoid_only_right_line: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_only_center: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_step: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_w_linear: # only for continuous actions + beta_0: 3 + beta_1: -0.1 + penal: 0 + min_reward: 1_000 + highest_reward: 100 + +carla_environments: + follow_lane: + env_name: CarlaEnv-v0 + car: model1 + #sync_mode: True + weather: ClearNoon #dynamic, argparse + traffic_pedestrians: False + city_lights: False + car_lights: False + estimated_steps: 500 + save_episodes: 20 + save_every_step: 50 + filter: vehicle.* + generation: "2" + rolename: "hero" #name + gamma: 2.2 #for camera + sync: True #syncronous mode or async + waypoints_meters: 5 #distance between waypoints in meters + waypoints_init: 1 #Town07: (20, -1, 945) 951, #Town01: 879 crash testing, 839 init straight lane + waypoints_target: 1249 #Town07: (57, -1, 1249), #Town01: 959, 961 + waypoints_road_id: 8 # Town07: (20)-920-21-39-57, #Town01: 8 + max_target_waypoint_distance: 3 # distance to finish waypoint + waypoints_lane_id: lane-1 # lane-1 (right lane from south to north), lane1 + town: Town04_Opt #Town01, Town02, Town03, Town04, Town05, Town06, Town07, Town10HD, Town11 + # Town01_Opt, Town02_Opt, Town03_Opt, Town04_Opt, Town05_Opt, Town06_Opt, Town07_Opt, Town10HD_Opt + init_pose: + random_pose: False # random positions in the whole town + alternate_pose: True #random positions but only for selected poses in carla_town_positions->tonwXX->laneX->init_positions + init_pose_number: 0 + finish_pose_number: 0 + town_circuit: lateral_circuit + +carla_town_positions: + Town04: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town04_Opt: + lateral_circuit: #curves, right and left + lane-1: + init_positions: + 0: [-15.8, -267.1, -0.0, -0.0, 122.4, -0.0] + #0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + #1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + #2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + #3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #0: [32.3, 290.7, 0.0, 0.0, -44.1, 0.0] #x, y, z, pitch, yaw, roll + #1: [-399.6, 6.2, -0.0, -0.0, -176.0, 0.0] #x, y, z, pitch, yaw, roll + #2: [13.0, 310.3, -0.0, -0.0, -43.7, 0.0] #x, y, z, pitch, yaw, roll + #3: [-26.0, -250.6, -0.0, -0.0, 122.3, 0.0] #x, y, z, pitch, yaw, roll + #4: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #5: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + #6: [-380.9, -3.4, -0.0, 0.1, -93.2, 0.0] #x, y, z, pitch, yaw, roll + #7: [-15.9, -266.8, -0.0, -0.0, 121.3, 0.0] #x, y, z, pitch, yaw, roll + #8: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + finish_positions: + 0: [-384.6, -8.5, -0.0, -0.0, 94.5, -0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + highway_circuit: + lane-1: + init_positions: + #0: [111.8, 38.1, 10.4, -1.1, -0.1, -0.2] #x, y, z, pitch, yaw, roll + 0: [-415.7, 6.0, -0.0, 0.0, -178.8, 0.0] #init + 1: [-21.2, 344.9, -0.0, -0.0, -45.1, 0.0] #x, y, z, pitch, yaw, roll + 2: [404.2, 24.4, 0.0, -0.0, -79.5, 0.0] #x, y, z, pitch, yaw, roll + 3: [-0.1, -295.6, -0.0, -0.0, 116.7, -0.0] #x, y, z, pitch, yaw, roll + 4: [410.947, -14.67, 0.2819, 0.0, -79.30, 0.0] + finish_positions: + #0: [-2.5, -290.8, -0.0, -0.0, 120.2, -0.0] #x, y, z, pitch, yaw, roll + 0: [-384.6, -8.5, -0.0, -0.0, 94.5, -0.0] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + + Town07_Opt: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] + #0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + + Town07: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + Town01_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town02_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town03_Opt: + town_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town05_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town06_Opt: + town_circuit: + lane-1: + init_positions: + 0: [102.1, 234.0, 0, 0, -14.9, 0] + 1: [479.2, 132.6, 0, -0, -24.3, -0] + 2: [87.7, 132.9, 0, 0, -11.1, 0] + finish_positions: + 0: [479.2, 132.6, 0, -0, -24.3, -0] + lane1: + init_positions: + finish_positions: + Town10HD_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town11: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: diff --git a/rl_studio/config/config_training_followlane_dqn_carla_landau_lane2.yaml b/rl_studio/config/config_training_followlane_dqn_carla_landau_lane2.yaml new file mode 100644 index 000000000..20b832458 --- /dev/null +++ b/rl_studio/config/config_training_followlane_dqn_carla_landau_lane2.yaml @@ -0,0 +1,473 @@ +settings: + station: landau # landau + mode: training # training, retraining, inference + task: follow_lane_carla # follow_line_gazebo, follow_lane_gazebo, autoparking_gazebo + algorithm: dqn # qlearn, dqn, ddpg, ppo + simulator: carla # openai, carla, gazebo, sumo + environment_set: carla_environments # gazebo_environments, carla_environments + env: follow_lane # simple, nurburgring, montreal, curves, simple_laser, manual, autoparking + agent: auto_carla # f1, autoparkingRL, auto_carla, mountain_car, robot_mesh, cartpole, turtlebot + actions: carla_discrete # continuous, simple, medium, hard, test, autoparking_simple, carla_discrete + states: sp7 #sp4 #image, sp1 (simplified perception with 1 point), sp3 (simplified perception with 3 points), spn (simplified perception with n points) + rewards: rewards_followlane_center_velocity_angle # follow_right_lane_only_center, follow_right_lane_center_v_step, follow_right_lane_center_v_w_linear + framework: TensorFlow # TensorFlow, Pytorch + total_episodes: 10_000 + training_time: 200 + models_dir: "./checkpoints" + logs_dir: "./logs" + metrics_dir: "./metrics" + recorder_carla_dir: "./recorders" + +ros: + ros_master_uri: "11311" + gazebo_master_uri: "11345" + +carla: + carla_server: localhost + carla_client: 4356 + traffic_manager_port: 4637 + +retraining: + dqn: + retrain_dqn_tf_model_name: "20230131-212216_Circuit-simple_States-sp1_Actions-simple_BESTLAP_Rewards-follow_right_lane_only_center_epsilon-0.459_epoch-2585_step-943_reward-4700_DQN_sp_16x16.h5" + ddpg: + retrain_ddpg_tf_actor_model_name: "20230111_DDPG_Actor_conv2d32x64_Critic_conv2d32x64_BESTLAP_ACTOR_Max-69_Epoch-4_State-image_Actions-continuous_Rewards-follow_right_lane_center_v_w_linear_inTime-20230111-200026.h5" + retrain_ddpg_tf_critic_model_name: "20230111_DDPG_Actor_conv2d32x64_Critic_conv2d32x64_BESTLAP_CRITIC_Max-69_Epoch-4_State-image_Actions-continuous_Rewards-follow_right_lane_center_v_w_linear_inTime-20230111-200026.h5" + +inference: + ddpg: + inference_ddpg_tf_actor_model_name: + inference_ddpg_tf_critic_model_name: + dqn: + inference_dqn_tf_model_name: + +algorithm: + dqn: + #alpha: 0.95 + gamma: 0.99 # future values in the long term reward. When close to 1, we reward long term + epsilon: 0.99 # in Retraining mode is convenient to reduce, i.e. 0.45 + epsilon_discount: 0.9986 + epsilon_min: 0.00 + model_name: SEQ_32x32 + replay_memory_size: 10_000 # How many last steps to keep for model training + min_replay_memory_size: 1000 #1000 # Minimum number of steps in a memory to start training + minibatch_size: 64 # How many steps (samples) to use for training + update_target_every: 5 + memory_fraction: 0.20 + #buffer_capacity: 100_000 + #batch_size: 64 + ddpg: + gamma: 0.99 + tau: 0.005 + std_dev: 0.2 + model_name: DDPG_Actor_conv2d32x64_Critic_conv2d32x64 + replay_memory_size: 50_000 + memory_fraction: 0.20 + critic_lr: 0.002 + actor_lr: 0.001 + buffer_capacity: 100_000 + batch_size: 64 + +agents: + auto_carla: + targets: + target_vel: 10 + camera_params: + width: 640 + height: 480 + center_image: 320 + raw_image: False + image_resizing: 100 + new_image_size: 32 + num_regions: 23 # this should be named states!!! + lower_limit: 220 + +states: # should be named as points of percepction (states name is wrong) + image: + 0: [50, 60, 70, 80] # try up to 4 points + sp1: + 0: [50] + sp3: + 0: [5, 15, 22] + sp4: + 0: [50, 60, 70, 80] #[50, 70, 90, 120] #[30, 50, 70, 90] #[90, 130, 170, 210] #[30, 60, 100, 120] + sp7: + 0: [30, 40, 50, 60, 70, 80, 90] #[30, 35, 40, 45, 50, 55, 60] + spn: + 0: [10] + states_entry: lane_detector_pytorch # only_right_line, between_right_left_lines, lane_detector_pytorch + +actions: + carla_discrete: + 0: [0.55, 0.0] #straight forward + 1: [0.3, 0.1] + 2: [0.3, 0.15] + 3: [0.3, -0.1] + 4: [0.3, -0.15] + 5: [0.35, 0.07] + 6: [0.35, 0.09] + 7: [0.35, -0.07] + 8: [0.35, -0.09] + 9: [0.4, 0.03] + 10: [0.4, 0.05] + 11: [0.4, 0.07] + 12: [0.4, -0.03] + 13: [0.4, -0.05] + 14: [0.4, -0.07] + 15: [0.45, 0.03] + 16: [0.45, 0.05] + 17: [0.45, -0.03] + 18: [0.45, -0.05] + 19: [0.50, 0.01] + 20: [0.5, 0.03] + 21: [0.5, -0.01] + 22: [0.5, -0.03] + #0: [0.5, 0.0] #straight forward + #1: [0.2, 0.1] + #2: [0.2, 0.2] + #3: [0.2, -0.1] + #4: [0.2, -0.2] + #5: [0.25, 0.07] + #6: [0.25, 0.1] + #7: [0.25, -0.07] + #8: [0.25, -0.1] + #9: [0.3, 0.03] + #10: [0.3, 0.05] + #11: [0.3, 0.07] + #12: [0.3, -0.03] + #13: [0.3, -0.05] + #14: [0.3, -0.7] + #15: [0.35, 0.03] + #16: [0.35, 0.05] + #17: [0.35, -0.03] + #18: [0.35, -0.05] + #19: [0.45, 0.01] + #20: [0.45, 0.03] + #21: [0.45, -0.01] + #22: [0.45, -0.03] + #0: [0.6, 0.0] #straight forward + #1: [0.2, 0.1] + #2: [0.2, 0.2] + #3: [0.2, -0.1] + #4: [0.2, -0.2] + #5: [0.3, 0.07] + #6: [0.3, 0.1] + #7: [0.3, -0.07] + #8: [0.3, -0.1] + #9: [0.4, 0.03] + #10: [0.4, 0.05] + #11: [0.4, 0.07] + #12: [0.4, -0.03] + #13: [0.4, -0.05] + #14: [0.4, -0.7] + #15: [0.5, 0.03] + #16: [0.5, 0.05] + #17: [0.5, -0.03] + #18: [0.5, -0.05] + #19: [0.55, 0.01] + #20: [0.55, 0.03] + #21: [0.55, -0.01] + #22: [0.55, -0.03] + #0: [0.5, -0.03] #turn left + #1: [0.55, 0.0] #straight forward + #2: [0.5, 0.03] #turn right + #3: [0.45, -0.07] #turn left hard + #4: [0.45, 0.07] #turn right hard + #5: [0.4, -0.1] #turn left + #6: [0.4, 0.1] #straight forward + continuous: + v: [0.4, 0.4] #throttle + w: [-0.07000, 0.07000] #steering + simple: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + medium: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1, 1.5] + 4: [1, -1.5] + hard: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1.5, 1] + 4: [1.5, -1] + 5: [1, -1.5] + 6: [1, -1.5] + test: + 0: [0, 0] + +rewards: + rewards_followlane_center_velocity_angle: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + rewards_sigmoid_only_right_line: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_only_center: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_step: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_w_linear: # only for continuous actions + beta_0: 3 + beta_1: -0.1 + penal: 0 + min_reward: 1_000 + highest_reward: 100 + +carla_environments: + follow_lane: + env_name: CarlaEnv-v0 + car: model1 + #sync_mode: True + weather: ClearNoon #dynamic, argparse + traffic_pedestrians: False + city_lights: False + car_lights: False + estimated_steps: 700 + save_episodes: 10 + save_every_step: 50 + filter: vehicle.* + generation: "2" + rolename: "hero" #name + gamma: 2.2 #for camera + sync: True #syncronous mode or async + waypoints_meters: 5 #distance between waypoints in meters + waypoints_init: 1 #Town07: (20, -1, 945) 951, #Town01: 879 crash testing, 839 init straight lane + waypoints_target: 1249 #Town07: (57, -1, 1249), #Town01: 959, 961 + waypoints_road_id: 8 # Town07: (20)-920-21-39-57, #Town01: 8 + max_target_waypoint_distance: 3 # distance to finish waypoint + waypoints_lane_id: lane1 # lane-1 (right lane from south to north), lane1 + town: Town04_Opt #Town01, Town02, Town03, Town04, Town05, Town06, Town07, Town10HD, Town11 + # Town01_Opt, Town02_Opt, Town03_Opt, Town04_Opt, Town05_Opt, Town06_Opt, Town07_Opt, Town10HD_Opt + init_pose: + random_pose: False # random positions in the whole town + alternate_pose: True #random positions but only for selected poses in carla_town_positions->tonwXX->laneX->init_positions + init_pose_number: 0 + finish_pose_number: 0 + town_circuit: lateral_circuit + +carla_town_positions: + Town04: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town04_Opt: + lateral_circuit: #up and down in lateral lanes + lane-1: + init_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #0: [32.3, 290.7, 0.0, 0.0, -44.1, 0.0] #x, y, z, pitch, yaw, roll + #1: [-399.6, 6.2, -0.0, -0.0, -176.0, 0.0] #x, y, z, pitch, yaw, roll + #2: [13.0, 310.3, -0.0, -0.0, -43.7, 0.0] #x, y, z, pitch, yaw, roll + #3: [-26.0, -250.6, -0.0, -0.0, 122.3, 0.0] #x, y, z, pitch, yaw, roll + #4: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #5: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + #6: [-380.9, -3.4, -0.0, 0.1, -93.2, 0.0] #x, y, z, pitch, yaw, roll + #7: [-15.9, -266.8, -0.0, -0.0, 121.3, 0.0] #x, y, z, pitch, yaw, roll + #8: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + finish_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + 0: [-381.2, 2.3, -0, -0.1, -88.6, 0.0] + finish_positions: + 0: [-39.1, -226.1, -0, 0.0, -54, 0.0] + highway_circuit: + lane-1: + init_positions: + #0: [111.8, 38.1, 10.4, -1.1, -0.1, -0.2] #x, y, z, pitch, yaw, roll + 0: [-415.7, 6.0, -0.0, 0.0, -178.8, 0.0] #init + 1: [-21.2, 344.9, -0.0, -0.0, -45.1, 0.0] #x, y, z, pitch, yaw, roll + 2: [404.2, 24.4, 0.0, -0.0, -79.5, 0.0] #x, y, z, pitch, yaw, roll + 3: [-0.1, -295.6, -0.0, -0.0, 116.7, -0.0] #x, y, z, pitch, yaw, roll + 4: [410.947, -14.67, 0.2819, 0.0, -79.30, 0.0] + finish_positions: + #0: [-2.5, -290.8, -0.0, -0.0, 120.2, -0.0] #x, y, z, pitch, yaw, roll + 0: [-384.6, -8.5, -0.0, -0.0, 94.5, -0.0] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + + Town07_Opt: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] + #0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + + Town07: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + Town01_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town02_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town03_Opt: + town_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town05_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town06_Opt: + town_circuit: + lane-1: + init_positions: + 0: [102.1, 234.0, 0, 0, -14.9, 0] + 1: [479.2, 132.6, 0, -0, -24.3, -0] + 2: [87.7, 132.9, 0, 0, -11.1, 0] + finish_positions: + 0: [479.2, 132.6, 0, -0, -24.3, -0] + lane1: + init_positions: + finish_positions: + Town10HD_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town11: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: diff --git a/rl_studio/config/config_training_followlane_dqn_carla_sb3.yaml b/rl_studio/config/config_training_followlane_dqn_carla_sb3.yaml new file mode 100644 index 000000000..628011e71 --- /dev/null +++ b/rl_studio/config/config_training_followlane_dqn_carla_sb3.yaml @@ -0,0 +1,391 @@ +settings: + mode: training # training, retraining, inference + task: follow_lane_carla # follow_line_gazebo, follow_lane_gazebo, autoparking_gazebo + algorithm: dqn # qlearn, dqn, ddpg, ppo + simulator: carla # openai, carla, gazebo, sumo + environment_set: carla_environments # gazebo_environments, carla_environments + env: follow_lane # simple, nurburgring, montreal, curves, simple_laser, manual, autoparking + agent: auto_carla # f1, autoparkingRL, auto_carla, mountain_car, robot_mesh, cartpole, turtlebot + actions: carla_discrete # continuous, simple, medium, hard, test, autoparking_simple, carla_discrete + states: sp4 #sp4 #image, sp1 (simplified perception with 1 point), sp3 (simplified perception with 3 points), spn (simplified perception with n points) + rewards: rewards_sigmoid_only_right_line # follow_right_lane_only_center, follow_right_lane_center_v_step, follow_right_lane_center_v_w_linear + framework: sb3 # TensorFlow, Pytorch, sb3 + total_episodes: 5000 + training_time: 16 + models_dir: "./checkpoints" + logs_dir: "./logs" + metrics_dir: "./metrics" + recorder_carla_dir: "./recorders" + +ros: + ros_master_uri: "11311" + gazebo_master_uri: "11345" + +carla: + carla_server: localhost + carla_client: 2000 + +retraining: + dqn: + retrain_dqn_tf_model_name: "20230131-212216_Circuit-simple_States-sp1_Actions-simple_BESTLAP_Rewards-follow_right_lane_only_center_epsilon-0.459_epoch-2585_step-943_reward-4700_DQN_sp_16x16.h5" + ddpg: + retrain_ddpg_tf_actor_model_name: "20230111_DDPG_Actor_conv2d32x64_Critic_conv2d32x64_BESTLAP_ACTOR_Max-69_Epoch-4_State-image_Actions-continuous_Rewards-follow_right_lane_center_v_w_linear_inTime-20230111-200026.h5" + retrain_ddpg_tf_critic_model_name: "20230111_DDPG_Actor_conv2d32x64_Critic_conv2d32x64_BESTLAP_CRITIC_Max-69_Epoch-4_State-image_Actions-continuous_Rewards-follow_right_lane_center_v_w_linear_inTime-20230111-200026.h5" + +inference: + ddpg: + inference_ddpg_tf_actor_model_name: + inference_ddpg_tf_critic_model_name: + dqn: + inference_dqn_tf_model_name: + +algorithm: + dqn: + alpha: 0.8 + gamma: 0.9 + epsilon: 0.99 # in Retraining mode is convenient to reduce, i.e. 0.45 + epsilon_discount: 0.9986 + epsilon_min: 0.00 + model_name: DQN_sp_16x16 #DQN_im_32x64 + replay_memory_size: 50_000 + min_replay_memory_size: 1000 + minibatch_size: 64 + update_target_every: 5 + memory_fraction: 0.20 + buffer_capacity: 100_000 + batch_size: 64 + ddpg: + gamma: 0.99 + tau: 0.005 + std_dev: 0.2 + model_name: DDPG_Actor_conv2d32x64_Critic_conv2d32x64 + replay_memory_size: 50_000 + memory_fraction: 0.20 + critic_lr: 0.002 + actor_lr: 0.001 + buffer_capacity: 100_000 + batch_size: 64 + +agents: + auto_carla: + camera_params: + width: 640 + height: 480 + center_image: 320 + raw_image: False + image_resizing: 100 + new_image_size: 32 + num_regions: 16 + lower_limit: 220 + +states: + image: + 0: [30, 40, 50, 60] # try up to 4 points + sp1: + 0: [50] + sp3: + 0: [5, 15, 22] + sp4: + 0: [30, 40, 50, 60] #[50, 70, 90, 120] #[30, 50, 70, 90] #[90, 130, 170, 210] #[30, 60, 100, 120] + sp5: + 0: [3, 5, 10, 15, 20] + spn: + 0: [10] + states_entry: only_right_line # only_right_line, between_right_left_lines, lane_detector_pytorch + +actions: + carla_discrete: + 0: [0.5, -0.03] #turn left + 1: [0.55, 0.0] #straight forward + 2: [0.5, 0.03] #turn right + 3: [0.45, -0.07] #turn left hard + 4: [0.45, 0.07] #turn right hard + #5: [0.4, -0.1] #turn left + #6: [0.4, 0.1] #straight forward + continuous: + v: [0.4, 0.4] #throttle + w: [-0.07000, 0.07000] #steering + simple: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + medium: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1, 1.5] + 4: [1, -1.5] + hard: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1.5, 1] + 4: [1.5, -1] + 5: [1, -1.5] + 6: [1, -1.5] + test: + 0: [0, 0] + +rewards: + rewards_sigmoid_only_right_line: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_only_center: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_step: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_w_linear: # only for continuous actions + beta_0: 3 + beta_1: -0.1 + penal: 0 + min_reward: 1_000 + highest_reward: 100 + +carla_environments: + follow_lane: + env_name: CarlaEnv-v0 + car: model1 + #sync_mode: True + weather: ClearNoon #dynamic, argparse + traffic_pedestrians: False + city_lights: False + car_lights: False + estimated_steps: 300 + save_episodes: 5 + save_every_step: 50 + filter: vehicle.* + generation: "2" + rolename: "hero" #name + gamma: 2.2 #for camera + sync: True #syncronous mode or async + waypoints_meters: 5 #distance between waypoints in meters + waypoints_init: 1 #Town07: (20, -1, 945) 951, #Town01: 879 crash testing, 839 init straight lane + waypoints_target: 1249 #Town07: (57, -1, 1249), #Town01: 959, 961 + waypoints_road_id: 8 # Town07: (20)-920-21-39-57, #Town01: 8 + max_target_waypoint_distance: 3 # distance to finish waypoint + waypoints_lane_id: lane-1 # lane-1 (right lane from south to north), lane1 + town: Town04_Opt #Town01, Town02, Town03, Town04, Town05, Town06, Town07, Town10HD, Town11 + # Town01_Opt, Town02_Opt, Town03_Opt, Town04_Opt, Town05_Opt, Town06_Opt, Town07_Opt, Town10HD_Opt + init_pose: + random_pose: False # random positions in the whole town + alternate_pose: True # random positions but only for selected poses in carla_town_positions->tonwXX->laneX->init_positions + init_pose_number: 0 + finish_pose_number: 0 + town_circuit: highway_circuit + +carla_town_positions: + Town04: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town04_Opt: + lateral_circuit: #up and down in lateral lanes + lane-1: + init_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #0: [32.3, 290.7, 0.0, 0.0, -44.1, 0.0] #x, y, z, pitch, yaw, roll + #1: [-399.6, 6.2, -0.0, -0.0, -176.0, 0.0] #x, y, z, pitch, yaw, roll + #2: [13.0, 310.3, -0.0, -0.0, -43.7, 0.0] #x, y, z, pitch, yaw, roll + #3: [-26.0, -250.6, -0.0, -0.0, 122.3, 0.0] #x, y, z, pitch, yaw, roll + #4: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #5: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + #6: [-380.9, -3.4, -0.0, 0.1, -93.2, 0.0] #x, y, z, pitch, yaw, roll + #7: [-15.9, -266.8, -0.0, -0.0, 121.3, 0.0] #x, y, z, pitch, yaw, roll + #8: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + finish_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + highway_circuit: + lane-1: + init_positions: + #0: [111.8, 38.1, 10.4, -1.1, -0.1, -0.2] #x, y, z, pitch, yaw, roll + 0: [-415.7, 6.0, -0.0, 0.0, -178.8, 0.0] #init + 1: [-21.2, 344.9, -0.0, -0.0, -45.1, 0.0] #x, y, z, pitch, yaw, roll + 2: [404.2, 24.4, 0.0, -0.0, -79.5, 0.0] #x, y, z, pitch, yaw, roll + 3: [-0.1, -295.6, -0.0, -0.0, 116.7, -0.0] #x, y, z, pitch, yaw, roll + 4: [410.947, -14.67, 0.2819, 0.0, -79.30, 0.0] + finish_positions: + #0: [-2.5, -290.8, -0.0, -0.0, 120.2, -0.0] #x, y, z, pitch, yaw, roll + 0: [-384.6, -8.5, -0.0, -0.0, 94.5, -0.0] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + + Town07_Opt: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] + #0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + + Town07: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + Town01_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town02_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town03_Opt: + town_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town05_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town06_Opt: + town_circuit: + lane-1: + init_positions: + 0: [102.1, 234.0, 0, 0, -14.9, 0] + 1: [479.2, 132.6, 0, -0, -24.3, -0] + 2: [87.7, 132.9, 0, 0, -11.1, 0] + finish_positions: + 0: [479.2, 132.6, 0, -0, -24.3, -0] + lane1: + init_positions: + finish_positions: + Town10HD_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town11: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: diff --git a/rl_studio/config/config_training_followlane_qlearn_carla.yaml b/rl_studio/config/config_training_followlane_qlearn_carla.yaml index 9ec99d912..fac771e15 100644 --- a/rl_studio/config/config_training_followlane_qlearn_carla.yaml +++ b/rl_studio/config/config_training_followlane_qlearn_carla.yaml @@ -1,5 +1,5 @@ settings: - mode: training # training, retraining + mode: retraining # training, retraining task: follow_lane_carla # follow_line_gazebo, follow_lane_gazebo algorithm: qlearn # qlearn simulator: carla # openai, carla, gazebo, sumo @@ -8,10 +8,10 @@ settings: agent: auto_carla # f1, autoparkingRL, auto_carla, mountain_car, robot_mesh, cartpole, turtlebot actions: carla_simple # simple, medium, hard, test, autoparking_simple states: sp4 # sp1 (simplified perception with 1 point), sp3 (simplified perception with 3 points), spn (simplified perception with n points) - rewards: follow_right_lane_only_center # + rewards: rewards_sigmoid_only_right_line # framework: _ - total_episodes: 3 - training_time: 6 + total_episodes: 1000 + training_time: 12 models_dir: "./checkpoints" logs_dir: "./logs" metrics_dir: "./metrics" @@ -27,7 +27,7 @@ carla: retraining: qlearn: - retrain_qlearn_model_name: "20230123-161229_Circuit-simple_States-sp1_Actions-simple_Rewards-follow_right_lane_only_center_epsilon-0.399_epoch-291_step-15001_reward-136707-qtable.npy" + retrain_qlearn_model_name: "20230715-171404_Circuit-Town04_Opt_States-sp4_Actions-carla_simple_Rewards-rewards_sigmoid_only_right_line_epsilon-0.779_epoch-181_step-301_reward-2987_QTABLE.pkl" inference: qlearn: @@ -43,30 +43,36 @@ algorithm: agents: auto_carla: camera_params: - width: 640 - height: 480 + width: 640 #1024 #640 + height: 480 #512 #480 center_image: 320 raw_image: False image_resizing: 100 new_image_size: 32 - num_regions: 16 + num_regions: 7 lower_limit: 220 -states: +states: # only values allowed: [10, 20, 30, 60, 80, 100, 130, 180, 200, 230] sp1: - 0: [50] + 0: [60] #100 works well sp4: - 0: [30, 60, 100, 120] + 0: [30, 40, 50, 60] #[50, 70, 90, 120] #[30, 50, 70, 90] #[90, 130, 170, 210] #[30, 60, 100, 120] sp5: 0: [3, 5, 10, 15, 20] spn: 0: [50, 120, 180] + states_entry: one_only_right_line # one_only_right_line, one_between_right_left_lines actions: carla_simple: - 0: [1, -0.2] - 1: [1, 0] - 2: [1, 0.2] + 0: [0.5, -0.03] #turn left + 1: [0.55, 0.0] #straight forward + 2: [0.5, 0.03] #turn right + 3: [0.45, -0.07] #turn left hard + 4: [0.45, 0.07] #turn right hard + #5: [0.4, -0.1] #turn left + #6: [0.4, 0.1] #straight forward + simple: 0: [2, -1] 1: [3, 0] @@ -89,6 +95,13 @@ actions: 0: [0, 0] rewards: + rewards_sigmoid_only_right_line: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 follow_right_lane_only_center: from_10: 10 from_02: 2 @@ -107,27 +120,234 @@ rewards: carla_environments: follow_lane: env_name: CarlaEnv-v0 - town: Town01 #Town01, Town02, Town03, Town04, Town05, Town10HD - # Town01_Opt, Town02_Opt, Town03_Opt, Town04_Opt, Town05_Opt, Town10HD_Opt car: model1 #sync_mode: True weather: ClearNoon #dynamic, argparse traffic_pedestrians: False city_lights: False car_lights: False - estimated_steps: 5 - save_episodes: 10 - save_every_step: 1000 - init_pose: - goal_pose: + estimated_steps: 300 + save_episodes: 5 + save_every_step: 50 filter: vehicle.* generation: "2" rolename: "hero" #name gamma: 2.2 #for camera sync: True #syncronous mode or async - alternate_pose: False waypoints_meters: 5 #distance between waypoints in meters - waypoints_init: 839 - waypoints_target: 959 #961 - waypoints_lane_id: -1 - waypoints_road_id: 8 + waypoints_init: 1 #Town07: (20, -1, 945) 951, #Town01: 879 crash testing, 839 init straight lane + waypoints_target: 1249 #Town07: (57, -1, 1249), #Town01: 959, 961 + waypoints_road_id: 8 # Town07: (20)-920-21-39-57, #Town01: 8 + max_target_waypoint_distance: 3 # distance to finish waypoint + waypoints_lane_id: lane-1 # lane-1 (right lane from south to north), lane1 + town: Town04_Opt #Town01, Town02, Town03, Town04, Town05, Town06, Town07, Town10HD, Town11 + # Town01_Opt, Town02_Opt, Town03_Opt, Town04_Opt, Town05_Opt, Town06_Opt, Town07_Opt, Town10HD_Opt + init_pose: + random_pose: False # random positions in the whole town + alternate_pose: True # random positions but only for selected poses in carla_town_positions->tonwXX->laneX->init_positions + init_pose_number: 0 + finish_pose_number: 0 + town_circuit: highway_circuit + +carla_town_positions: + Town04: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town04_Opt: + lateral_circuit: #up and down in lateral lanes + lane-1: + init_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #0: [32.3, 290.7, 0.0, 0.0, -44.1, 0.0] #x, y, z, pitch, yaw, roll + #1: [-399.6, 6.2, -0.0, -0.0, -176.0, 0.0] #x, y, z, pitch, yaw, roll + #2: [13.0, 310.3, -0.0, -0.0, -43.7, 0.0] #x, y, z, pitch, yaw, roll + #3: [-26.0, -250.6, -0.0, -0.0, 122.3, 0.0] #x, y, z, pitch, yaw, roll + #4: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + #5: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + #6: [-380.9, -3.4, -0.0, 0.1, -93.2, 0.0] #x, y, z, pitch, yaw, roll + #7: [-15.9, -266.8, -0.0, -0.0, 121.3, 0.0] #x, y, z, pitch, yaw, roll + #8: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + finish_positions: + 0: [22, -75.8, -0.0, -0.0, -58.1, 0.0] #x, y, z, pitch, yaw, roll + 1: [-103.1, -6.7, 9.3, 0.1, -133.0, -0.3] #x, y, z, pitch, yaw, roll + 2: [-25.8, 124.7, -0.0, -0.0, 131.7, -0.0] #x, y, z, pitch, yaw, roll + 3: [109.3, 48.9, 10.4, -1.7, 42.3, -0.4] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + highway_circuit: + lane-1: + init_positions: + #0: [111.8, 38.1, 10.4, -1.1, -0.1, -0.2] #x, y, z, pitch, yaw, roll + 0: [-415.7, 6.0, -0.0, 0.0, -178.8, 0.0] #init + 1: [-21.2, 344.9, -0.0, -0.0, -45.1, 0.0] #x, y, z, pitch, yaw, roll + 2: [404.2, 24.4, 0.0, -0.0, -79.5, 0.0] #x, y, z, pitch, yaw, roll + 3: [-0.1, -295.6, -0.0, -0.0, 116.7, -0.0] #x, y, z, pitch, yaw, roll + 4: [410.947, -14.67, 0.2819, 0.0, -79.30, 0.0] + finish_positions: + #0: [-2.5, -290.8, -0.0, -0.0, 120.2, -0.0] #x, y, z, pitch, yaw, roll + 0: [-384.6, -8.5, -0.0, -0.0, 94.5, -0.0] #x, y, z, pitch, yaw, roll + lane1: + init_positions: + finish_positions: + + Town07_Opt: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] + #0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + + Town07: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + Town01_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town02_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town03_Opt: + town_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town05_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town06_Opt: + town_circuit: + lane-1: + init_positions: + 0: [102.1, 234.0, 0, 0, -14.9, 0] + 1: [479.2, 132.6, 0, -0, -24.3, -0] + 2: [87.7, 132.9, 0, 0, -11.1, 0] + finish_positions: + 0: [479.2, 132.6, 0, -0, -24.3, -0] + lane1: + init_positions: + finish_positions: + Town10HD_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town11: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: diff --git a/rl_studio/config/config_training_followlane_qlearn_carla_landau.yaml b/rl_studio/config/config_training_followlane_qlearn_carla_landau.yaml new file mode 100644 index 000000000..54a46987a --- /dev/null +++ b/rl_studio/config/config_training_followlane_qlearn_carla_landau.yaml @@ -0,0 +1,306 @@ +settings: + mode: training # training, retraining + task: follow_lane_carla # follow_line_gazebo, follow_lane_gazebo + algorithm: qlearn # qlearn + simulator: carla # openai, carla, gazebo, sumo + environment_set: carla_environments # gazebo_environments, carla_environments + env: follow_lane # Town01, simple, nurburgring, montreal, curves, simple_laser, manual, autoparking + agent: auto_carla # f1, autoparkingRL, auto_carla, mountain_car, robot_mesh, cartpole, turtlebot + actions: carla_simple # simple, medium, hard, test, autoparking_simple + states: sp4 # sp1 (simplified perception with 1 point), sp3 (simplified perception with 3 points), spn (simplified perception with n points) + rewards: follow_right_lane_only_center # + framework: _ + total_episodes: 1000 + training_time: 12 + models_dir: "./checkpoints" + logs_dir: "./logs" + metrics_dir: "./metrics" + recorder_carla_dir: "./recorders" + +ros: + ros_master_uri: "11311" + gazebo_master_uri: "11345" + +carla: + carla_server: localhost + carla_client: + +retraining: + qlearn: + retrain_qlearn_model_name: "20230123-161229_Circuit-simple_States-sp1_Actions-simple_Rewards-follow_right_lane_only_center_epsilon-0.399_epoch-291_step-15001_reward-136707-qtable.npy" + +inference: + qlearn: + inference_qlearn_model_name: + +algorithm: + qlearn: + alpha: 0.7 + epsilon: 0.95 + epsilon_min: 0.05 + gamma: 0.9 + +agents: + auto_carla: + camera_params: + width: 640 + height: 480 + center_image: 320 + raw_image: False + image_resizing: 100 + new_image_size: 32 + num_regions: 8 + lower_limit: 220 + +states: # only values allowed: [10, 20, 30, 60, 80, 100, 130, 180, 200, 230] + sp1: + 0: [36] #100 works well + sp4: + 0: [90, 130, 170, 210] #[30, 60, 100, 120] + sp5: + 0: [3, 5, 10, 15, 20] + spn: + 0: [50, 120, 180] + points_per_line: one_only_right_line # + +actions: + carla_simple: + 0: [1, -0.2] + 1: [1, 0] + 2: [1, 0.2] + #3: [1, -0.1] + #4: [1, 0.1] + simple: + 0: [2, -1] + 1: [3, 0] + 2: [2, 1] + medium: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1, 1.5] + 4: [1, -1.5] + hard: + 0: [3, 0] + 1: [2, 1] + 2: [2, -1] + 3: [1.5, 1] + 4: [1.5, -1] + 5: [1, -1.5] + 6: [1, -1.5] + test: + 0: [0, 0] + +rewards: + follow_right_lane_only_center: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + follow_right_lane_center_v_step: + from_10: 10 + from_02: 2 + from_01: 1 + penal: -100 + min_reward: 5_000 + highest_reward: 100 + +carla_environments: + follow_lane: + env_name: CarlaEnv-v0 + car: model1 + #sync_mode: True + weather: ClearNoon #dynamic, argparse + traffic_pedestrians: False + city_lights: False + car_lights: False + estimated_steps: 100 + save_episodes: 5 + save_every_step: 50 + filter: vehicle.* + generation: "2" + rolename: "hero" #name + gamma: 2.2 #for camera + sync: True #syncronous mode or async + waypoints_meters: 5 #distance between waypoints in meters + waypoints_init: 1 #Town07: (20, -1, 945) 951, #Town01: 879 crash testing, 839 init straight lane + waypoints_target: 1249 #Town07: (57, -1, 1249), #Town01: 959, 961 + waypoints_road_id: 8 # Town07: (20)-920-21-39-57, #Town01: 8 + max_target_waypoint_distance: 3 # distance to finish waypoint + waypoints_lane_id: lane-1 # lane-1 (right lane from south to north), lane1 + town: Town07_Opt #Town01, Town02, Town03, Town04, Town05, Town06, Town07, Town10HD, Town11 + # Town01_Opt, Town02_Opt, Town03_Opt, Town04_Opt, Town05_Opt, Town06_Opt, Town07_Opt, Town10HD_Opt + init_pose: + random_pose: False # random positions in the whole town + alternate_pose: False # random positions but only for selected poses in carla_town_positions->tonwXX->laneX->init_positions + init_pose_number: 0 + finish_pose_number: 0 + town_circuit: mountain_circuit + +carla_town_positions: + Town04: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town07_Opt: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] + #0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + + Town07: + mountain_circuit: + lane-1: + init_positions: + 0: [73.7, -10, 0.3, 0.0, -62.5, 0.0] #x, y, z, pitch, yaw, roll + #1: [72.2, -38.7, 1.9, 4.4, 135.4, -0.3] #x, y, z, pitch, yaw, roll + #1: [71.6, -38.2, 1.9, 4.6, -132, 0] #x, y, z, pitch, yaw, roll + #2: [59.6, -70.8, 5.0, 4.7, -81.3, 0.0] #x, y, z, pitch, yaw, roll + #2: [61.3, -86.6, 6.3, 4.9, -79.1, 0.1] #x, y, z, pitch, yaw, roll + #3: [67.1, -112.6, 8.5, 4.5, -81.9, 0.0] #x, y, z, pitch, yaw, roll + 1: [61.1, -137.8, 9.3, -2.2, -123.4, 0.1] #x, y, z, pitch, yaw, roll + 2: [43.6, -193.1, 4.7, -4.5, -96.0, 0.1] #x, y, z, pitch, yaw, roll + #5: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] # end point also + #5: [19.5, -238.5, 0.5, -4.6, -155.9, -0.1] # end point also + 3: [-13.7, -243.7, 0, 4.5, -170.9, -0.2] # init point also + 4: [-65.5, -233.1, 4.4, 4.3, 118.5, -0.5] + #8: [-119, -217.4, 5.8, -5.4, -129.4, -0.4] + #8: [-119, -219.7, 5.5, -5.2, -125.4, -0.2] + 5: [-165.8, -247.4, 0.9, -5.0, -173.5, 0.2] + #6: [-201.6, -223, 0, -0, 91.6, -0] + #7: [-201.8, -173.4, 0, -0, 90.1, 0] + finish_positions: + 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + 1: [-201.8, -173.4, 0, -0, 90.1] + lane1: + init_positions: + 0: [-198.2, -170.6, 0, 0.0, -89.4, 0.0] + 1: [-198.3, -208.4, 0, -0.0, -89.6, 0.0] + 2: [-161.9, -243.1, 1.3, 5.1, 9.4, 0.0] + 3: [-127.2, -222.1, 5.1, 5.0, 52.1, -0.5] + #4: [-67.1, -223.6, 5.1, -4.6, -56, -0.5] + #5: [-19.9, -241.3, 0.4, -4.6, 12.7, -0.1] # also end point + 4: [-23.8, -242.5, 0.8, -4.7, 11.4, 0.0] # also end point + 5: [14.6, -237, 0.2, 4.5, 21.5, -0.2] # also init point + 6: [40.8, -185.2, 5.3, 4.6, 86.5, -0] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #8: [62.9, -108.4, 8.1, -4.8, 99.3, 0.2] + #9: [61.3, -45.5, 2.9, -4.5, 50.3, 0.4] # + #9: [58.9, -50, 3.3, -4.5, 58.9, 0.5] # + + finish_positions: + 0: [-66.2, -225.2, 0.3] + 1: [71.7, -10, 0.3] #longest finish. In alternate_pose, use it + Town01_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town02_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town03_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + + Town04_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town05_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town06_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town10HD_Opt: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: + Town11: + mountain_circuit: + lane-1: + init_positions: + finish_positions: + lane1: + init_positions: + finish_positions: diff --git a/rl_studio/config/config_training_followline_ddpg_f1_gazebo.yaml b/rl_studio/config/config_training_followline_ddpg_f1_gazebo.yaml index 7f4a60460..8d3f86153 100644 --- a/rl_studio/config/config_training_followline_ddpg_f1_gazebo.yaml +++ b/rl_studio/config/config_training_followline_ddpg_f1_gazebo.yaml @@ -6,8 +6,8 @@ settings: environment_set: gazebo_environments # gazebo_environments, carla_environments env: simple # simple, nurburgring, montreal, curves, simple_laser, manual, autoparking agent: f1 # f1, autoparkingRL, auto_carla, mountain_car, robot_mesh, cartpole, turtlebot - actions: continuous # continuous, simple, medium, hard, test, autoparking_simple - states: image #image, sp1 (simplified perception with 1 point), sp3 (simplified perception with 3 points), spn (simplified perception with n points) + actions: simple # continuous, simple, medium, hard, test, autoparking_simple + states: sp1 #image, sp1 (simplified perception with 1 point), sp3 (simplified perception with 3 points), spn (simplified perception with n points) rewards: followline_center # followline_center, followline_center_v_w_linear framework: TensorFlow # TensorFlow, Pytorch total_episodes: 5 @@ -15,6 +15,7 @@ settings: models_dir: "./checkpoints" logs_dir: "./logs" metrics_dir: "./metrics" + recorder_carla_dir: "./recorders" ros: ros_master_uri: "11311" diff --git a/rl_studio/docs/rls-diagram.png b/rl_studio/docs/rls-diagram.png new file mode 100644 index 000000000..fdb44c31e Binary files /dev/null and b/rl_studio/docs/rls-diagram.png differ diff --git a/rl_studio/envs/carla/__init__.py b/rl_studio/envs/carla/__init__.py index c8d2f9bbf..750d94d4e 100644 --- a/rl_studio/envs/carla/__init__.py +++ b/rl_studio/envs/carla/__init__.py @@ -6,6 +6,7 @@ class Carla: def __new__(cls, **environment): + print(f"llegamos a CarlaEnv \n") algorithm = environment["algorithm"] task = environment["task"] @@ -28,5 +29,53 @@ def __new__(cls, **environment): return FollowLaneQlearnStaticWeatherNoTraffic(**environment) + # ============================= + # FollowLane - DDPG - weather: static - traffic and pedestrians: No - TensorFlow + # ============================= + if ( + task == TasksType.FOLLOWLANECARLA.value + and algorithm == AlgorithmsType.DDPG.value + and weather != "dynamic" + and traffic_pedestrians is False + and framework == FrameworksType.TF.value + ): + from rl_studio.envs.carla.followlane.followlane_ddpg import ( + FollowLaneDDPGStaticWeatherNoTraffic, + ) + + return FollowLaneDDPGStaticWeatherNoTraffic(**environment) + + # ============================= + # FollowLane - DQN - weather: static - traffic and pedestrians: No - TensorFlow + # ============================= + if ( + task == TasksType.FOLLOWLANECARLA.value + and algorithm == AlgorithmsType.DQN.value + and weather != "dynamic" + and traffic_pedestrians is False + and framework == FrameworksType.TF.value + ): + from rl_studio.envs.carla.followlane.followlane_dqn import ( + FollowLaneDQNStaticWeatherNoTraffic, + ) + + return FollowLaneDQNStaticWeatherNoTraffic(**environment) + + # ============================= + # FollowLane - weather: static - traffic and pedestrians: No - Stable-Baselines3 + # ============================= + if ( + task == TasksType.FOLLOWLANECARLA.value + # and algorithm == AlgorithmsType.DQN.value + and weather != "dynamic" + and traffic_pedestrians is False + and framework == FrameworksType.STABLE_BASELINES3.value + ): + from rl_studio.envs.carla.followlane.followlane_sb3 import ( + FollowLaneStaticWeatherNoTrafficSB3, + ) + + return FollowLaneStaticWeatherNoTrafficSB3(**environment) + else: raise NoValidEnvironmentType(task) diff --git a/rl_studio/envs/carla/carla_env.py b/rl_studio/envs/carla/carla_env.py index 8767861ff..b22eef4df 100644 --- a/rl_studio/envs/carla/carla_env.py +++ b/rl_studio/envs/carla/carla_env.py @@ -29,9 +29,9 @@ class CarlaEnv(gym.Env): - def CFCCCSF__init__(self, **config): + def __init__(self, **config): """Constructor of the class.""" - print(f"in CarlaEnv\n") + # print(f"in CarlaEnv\n") self.carla_map = None self.client = None self.world = None @@ -210,7 +210,7 @@ def CFCCCSF__init__(self, **config): with open("/tmp/.carlalaunch_stdout.log", "w") as out, open( "/tmp/.carlalaunch_stderr.log", "w" ) as err: - print(f"hola estoy en CarlaEnv \n") + print(f"\nCarlaEnv has been launched in other terminal\n") # subprocess.Popen([carla_exec, "-prefernvidia"], stdout=out, stderr=err) # subprocess.Popen(["/home/jderobot/Documents/Projects/carla_simulator_0_9_13/CarlaUE4.sh", "-RenderOffScreen"], stdout=out, stderr=err) # subprocess.Popen(["/home/jderobot/Documents/Projects/carla_simulator_0_9_13/CarlaUE4.sh", "-RenderOffScreen", "-quality-level=Low"], stdout=out, stderr=err) @@ -230,7 +230,7 @@ def CFCCCSF__init__(self, **config): time.sleep(5) - def __init__(self, **config): + def __CWFDSSFFSFDinit__(self, **config): """Constructor of the class.""" self.carla_map = None self.client = None diff --git a/rl_studio/envs/carla/followlane/followlane_ddpg.py b/rl_studio/envs/carla/followlane/followlane_ddpg.py new file mode 100644 index 000000000..0ee85d774 --- /dev/null +++ b/rl_studio/envs/carla/followlane/followlane_ddpg.py @@ -0,0 +1,1869 @@ +from collections import Counter +import copy +from datetime import datetime, timedelta +import math +import os +import subprocess +import time +import sys + +import carla +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions +from cv_bridge import CvBridge +import cv2 +from geometry_msgs.msg import Twist +import matplotlib.pyplot as plt +from memory_profiler import profile +import numpy as np +import pygame +import random +import weakref +import rospy +from sensor_msgs.msg import Image +import torch +import torchvision + +from rl_studio.agents.utils import ( + print_messages, + render_params, +) +from rl_studio.envs.carla.followlane.followlane_env import FollowLaneEnv +from rl_studio.envs.carla.followlane.settings import FollowLaneCarlaConfig +from rl_studio.envs.carla.followlane.utils import AutoCarlaUtils + +from rl_studio.envs.carla.utils.bounding_boxes import BasicSynchronousClient +from rl_studio.envs.carla.utils.visualize_multiple_sensors import ( + DisplayManager, + SensorManager, + CustomTimer, +) + +from rl_studio.envs.carla.utils.global_route_planner import ( + GlobalRoutePlanner, +) + +correct_normalized_distance = { + 20: -0.07, + 30: -0.1, + 40: -0.13, + 50: -0.17, + 60: -0.2, + 70: -0.23, + 80: -0.26, + 90: -0.3, + 100: -0.33, + 110: -0.36, + 120: -0.4, + 130: -0.42, + 140: -0.46, + 150: -0.49, + 160: -0.52, + 170: -0.56, + 180: -0.59, + 190: -0.62, + 200: -0.65, + 210: -0.69, + 220: -0.72, +} +correct_pixel_distance = { + 20: 343, + 30: 353, + 40: 363, + 50: 374, + 60: 384, + 70: 394, + 80: 404, + 90: 415, + 100: 425, + 110: 436, + 120: 446, + 130: 456, + 140: 467, + 150: 477, + 160: 488, + 170: 498, + 180: 508, + 190: 518, + 200: 528, + 210: 540, + 220: 550, +} + + +class FollowLaneDDPGStaticWeatherNoTraffic(FollowLaneEnv): + def __init__(self, **config): + ## --------------- init env + FollowLaneEnv.__init__(self, **config) + ## --------------- init class variables + FollowLaneCarlaConfig.__init__(self, **config) + + self.timer = CustomTimer() + + self.client = carla.Client( + config["carla_server"], + config["carla_client"], + ) + self.client.set_timeout(3.0) + print(f"\n maps in carla 0.9.13: {self.client.get_available_maps()}\n") + + """ + self.world = self.client.get_world() + self.world = self.client.load_world(config["town"]) + settings = self.world.get_settings() + settings.fixed_delta_seconds = 0.05 + if config["sync"]: + settings.synchronous_mode = True + else: + settings.synchronous_mode = False + self.world.apply_settings(settings) + + """ + + self.world = self.client.load_world(config["town"]) + if config["town"] == "Town07_Opt": + self.world.unload_map_layer(carla.MapLayer.Buildings) + self.world.unload_map_layer(carla.MapLayer.Decals) + self.world.unload_map_layer(carla.MapLayer.Foliage) + self.world.unload_map_layer(carla.MapLayer.Particles) + self.world.unload_map_layer(carla.MapLayer.Props) + + self.original_settings = self.world.get_settings() + + # TODO: si algo se jode hay que quitar esta linea + self.traffic_manager = self.client.get_trafficmanager(8000) + settings = self.world.get_settings() + settings.fixed_delta_seconds = 0.1 # 0.05 + if config["sync"]: + # TODO: si algo se jode hay que quitar esta linea + self.traffic_manager.set_synchronous_mode(True) + # settings.synchronous_mode = True ####OJOOO + else: + self.traffic_manager.set_synchronous_mode(False) + # settings.synchronous_mode = False ###OJOJOOO + + self.world.apply_settings(settings) + + ### Weather + weather = carla.WeatherParameters( + cloudiness=70.0, precipitation=0.0, sun_altitude_angle=70.0 + ) + self.world.set_weather(weather) + + """este no funciona + self.world = self.client.load_world(config["town"]) + # self.original_settings = self.world.get_settings() + # self.traffic_manager = self.client.get_trafficmanager(8000) + settings = self.world.get_settings() + settings.fixed_delta_seconds = 0.05 + if config["sync"]: + settings.synchronous_mode = True + else: + settings.synchronous_mode = False + self.world.apply_settings(settings) + """ + + ## -- display manager + # self.display_manager = DisplayManager( + # grid_size=[2, 2], + # window_size=[900, 600], + # ) + + ## --------------- Blueprint --------------- + self.blueprint_library = self.world.get_blueprint_library() + ## --------------- Car --------------- + self.vehicle = self.world.get_blueprint_library().filter("vehicle.*")[0] + self.car = None + ## --------------- collision sensor --------------- + self.colsensor = self.world.get_blueprint_library().find( + "sensor.other.collision" + ) + self.collision_hist = [] + self.col_sensor = None + ## --------------- Lane invasion sensor --------------- + self.laneinvsensor = self.world.get_blueprint_library().find( + "sensor.other.lane_invasion" + ) + self.lane_changing_hist = [] + self.lane_sensor = None + ## --------------- Obstacle sensor --------------- + self.obstsensor = self.world.get_blueprint_library().find( + "sensor.other.obstacle" + ) + self.obstacle_hist = [] + self.obstacle_sensor = None + ## --------------- RGB camera --------------- + self.rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + self.rgb_cam.set_attribute("image_size_x", f"{self.width}") + self.rgb_cam.set_attribute("image_size_y", f"{self.height}") + self.rgb_cam.set_attribute("fov", f"110") + self.sensor_camera_rgb = None + self.front_rgb_camera = None + ## --------------- RedMask Camera --------------- + self.red_mask_cam = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + self.red_mask_cam.set_attribute("image_size_x", f"{self.width}") + self.red_mask_cam.set_attribute("image_size_y", f"{self.height}") + self.red_mask_cam.set_attribute("fov", f"110") + self.sensor_camera_red_mask = None + self.front_red_mask_camera = None + ## --------------- BEV camera --------------- + """ + self.birdview_producer = BirdViewProducer( + self.client, # carla.Client + target_size=PixelDimensions(width=100, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AREA_ONLY, + ) + self.bev_cam = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + self.bev_cam.set_attribute("image_size_x", f"{self.width}") + self.bev_cam.set_attribute("image_size_y", f"{self.height}") + self.front_camera_bev = None + self.front_camera_bev_mask = None + """ + ## --------------- Segmentation Camera --------------- + self.segm_cam = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + self.segm_cam.set_attribute("image_size_x", f"{self.width}") + self.segm_cam.set_attribute("image_size_y", f"{self.height}") + self.sensor_camera_segmentation = None + self.segmentation_cam = None + + ## --------------- LaneDetector Camera --------------- + # self.lanedetector_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # self.lanedetector_cam.set_attribute("image_size_x", f"{self.width}") + # self.lanedetector_cam.set_attribute("image_size_y", f"{self.height}") + # self.lanedetector_cam.set_attribute("fov", f"110") + # self.sensor_camera_lanedetector = None + # self.front_lanedetector_camera = None + + # self._detector = LaneDetector("models/fastai_torch_lane_detector_model.pth") + # torch.cuda.empty_cache() + # self.model = torch.load("models/fastai_torch_lane_detector_model.pth") + # self.model.eval() + + ## --------------- more --------------- + self.perfect_distance_pixels = None + self.perfect_distance_normalized = None + # self._control = carla.VehicleControl() + self.params = {} + self.target_waypoint = None + + self.spectator = self.world.get_spectator() + # self.spectator = None + self.actor_list = [] + self.is_finish = None + self.dist_to_finish = None + self.batch = [] + + ##################################################################################### + # + # RESET + # + ##################################################################################### + + def reset(self): + if self.state_space == "image": + return self.reset_image() + else: + return self.reset_sp() + + def reset_image(self): + """ + image + + """ + + if self.sensor_camera_rgb is not None: + self.destroy_all_actors_apply_batch() + + self.col_sensor = None + self.sensor_camera_rgb = None + self.sensor_camera_red_mask = None + self.front_camera_bev = None + self.front_camera_bev_mask = None + self.sensor_camera_segmentation = None + self.sensor_camera_lanedetector = None + + self.lane_sensor = None + self.obstacle_sensor = None + self.collision_hist = [] + self.lane_changing_hist = [] + self.obstacle_hist = [] + self.actor_list = [] + + ## --- Car + waypoints_town = self.world.get_map().generate_waypoints(5.0) + init_waypoint = waypoints_town[self.waypoints_init + 1] + filtered_waypoints = self.draw_waypoints( + waypoints_town, + self.waypoints_init, + self.waypoints_target, + self.waypoints_lane_id, + 2000, + ) + self.target_waypoint = filtered_waypoints[-1] + + ## ---- random init position in the whole Town: actually for test functioning porposes + if self.random_pose: + self.setup_car_random_pose() + ## -- Always same init position in a circuit predefined + elif self.alternate_pose is False: + self.setup_car_pose(self.start_alternate_pose, init=self.init_pose_number) + + ## -- Same circuit, but random init positions + else: + # self.setup_car_alternate_pose(self.start_alternate_pose) + self.setup_car_pose(self.start_alternate_pose) + + ## --- Cameras + + ## ---- RGB + # self.setup_rgb_camera() + self.setup_rgb_camera_weakref() + # self.setup_semantic_camera() + while self.sensor_camera_rgb is None: + time.sleep(0.01) + + ## ---- Red Mask + self.setup_red_mask_camera_weakref() + while self.sensor_camera_red_mask is None: + time.sleep(0.01) + + ## --- SEgmentation camera + self.setup_segmentation_camera_weakref() + # self.setup_segmentation_camera() + while self.sensor_camera_segmentation is None: + time.sleep(0.01) + + # ## ---- LAneDetector + # self.setup_lane_detector_camera_weakref() + # while self.sensor_camera_lane_detector is None: + # time.sleep(0.01) + + ## --- Detectors Sensors + self.setup_lane_invasion_sensor_weakref() + + """ apply_batch_sync for pedestrians and many cars """ + ## --- apply_batch_sync + # for response in self.client.apply_batch_sync(self.batch, True): + # if response.error: + # print(f"{response.error = }") + # logging.error(response.error) + # else: + # self.actor_list.append(response.actor_id) + + # NO results = client.apply_batch_sync(self.batch, True) + # for i in range(len(results)): + # if results[i].error: + # logging.error(results[i].error) + # else: + # self.actor_list[i]["con"] = results[i].actor_id + + time.sleep(1) + + ## Autopilot + # self.car.set_autopilot(True) + self.car.apply_control(carla.VehicleControl(throttle=0.0, brake=0.0)) + # self.setup_spectator() + + ########### --- calculating STATES + mask = self.simplifiedperception.preprocess_image(self.front_red_mask_camera) + # right_line_in_pixels = self.simplifiedperception.calculate_right_line( + # mask, self.x_row + # ) + + # pixels_in_state = mask.shape[1] / self.num_regions + # states = [ + # int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + # ] + + # states = np.array(self.convert_segment_image_to_mask(self.segmentation_cam)) + states = np.array( + self.simplifiedperception.resize_and_trimming_right_line_mask(mask) + ) + states_size = states.shape + + # print(f"{states =} and {states_size =}") + return states, states_size + + ############################################################################# + # Reset Simplified perceptcion + ############################################################################# + + def reset_sp(self): + """ + simplified perception + + """ + + if self.sensor_camera_rgb is not None: + self.destroy_all_actors_apply_batch() + + self.col_sensor = None + self.sensor_camera_rgb = None + self.sensor_camera_red_mask = None + self.front_camera_bev = None + self.front_camera_bev_mask = None + self.sensor_camera_segmentation = None + self.sensor_camera_lanedetector = None + + self.lane_sensor = None + self.obstacle_sensor = None + self.collision_hist = [] + self.lane_changing_hist = [] + self.obstacle_hist = [] + self.actor_list = [] + + ## --- Car + waypoints_town = self.world.get_map().generate_waypoints(5.0) + init_waypoint = waypoints_town[self.waypoints_init + 1] + filtered_waypoints = self.draw_waypoints( + waypoints_town, + self.waypoints_init, + self.waypoints_target, + self.waypoints_lane_id, + 2000, + ) + self.target_waypoint = filtered_waypoints[-1] + + ## ---- random init position in the whole Town: actually for test functioning porposes + if self.random_pose: + self.setup_car_random_pose() + ## -- Always same init position in a circuit predefined + elif self.alternate_pose is False: + self.setup_car_pose(self.start_alternate_pose, init=self.init_pose_number) + + ## -- Same circuit, but random init positions + else: + # self.setup_car_alternate_pose(self.start_alternate_pose) + self.setup_car_pose(self.start_alternate_pose) + + ## --- Cameras + + ## ---- RGB + # self.setup_rgb_camera() + self.setup_rgb_camera_weakref() + # self.setup_semantic_camera() + while self.sensor_camera_rgb is None: + time.sleep(0.01) + + ## ---- Red Mask + self.setup_red_mask_camera_weakref() + while self.sensor_camera_red_mask is None: + time.sleep(0.01) + + ## --- SEgmentation camera + self.setup_segmentation_camera_weakref() + # self.setup_segmentation_camera() + while self.sensor_camera_segmentation is None: + time.sleep(0.01) + + # ## ---- LAneDetector + # self.setup_lane_detector_camera_weakref() + # while self.sensor_camera_lane_detector is None: + # time.sleep(0.01) + + ## --- Detectors Sensors + self.setup_lane_invasion_sensor_weakref() + + time.sleep(1) + + ## Autopilot + # self.car.set_autopilot(True) + self.car.apply_control(carla.VehicleControl(throttle=0.0, brake=0.0)) + # self.setup_spectator() + + ########### --- calculating STATES + mask = self.simplifiedperception.preprocess_image(self.front_red_mask_camera) + right_line_in_pixels = self.simplifiedperception.calculate_right_line( + mask, self.x_row + ) + + pixels_in_state = mask.shape[1] / self.num_regions + states = [ + int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + ] + + states_size = len(states) + + # print(f"{states =} and {states_size =}") + return states, states_size + + ######################################################## + # waypoints, car pose + # + ######################################################## + + def draw_waypoints(self, spawn_points, init, target, lane_id, life_time): + """ + WArning: target always has to be the last waypoint, since it is the FINISH + """ + filtered_waypoints = [] + i = init + for waypoint in spawn_points[init + 1 : target + 2]: + filtered_waypoints.append(waypoint) + string = f"[{waypoint.road_id},{waypoint.lane_id},{i}]" + if waypoint.lane_id == lane_id: + if i != target: + self.world.debug.draw_string( + waypoint.transform.location, + f"X - {string}", + draw_shadow=False, + color=carla.Color(r=0, g=255, b=0), + life_time=life_time, + persistent_lines=True, + ) + else: + self.world.debug.draw_string( + waypoint.transform.location, + f"X - {string}", + draw_shadow=False, + color=carla.Color(r=255, g=0, b=0), + life_time=life_time, + persistent_lines=True, + ) + i += 1 + + return filtered_waypoints + + # @profile + def setup_car_pose(self, init_positions, init=None): + if init is None: + pose_init = np.random.randint(0, high=len(init_positions)) + else: + pose_init = init + + # print(f"{pose_init =}") + # print(f"{random_init = }") + # print(f"{self.start_alternate_pose = }") + # print(f"{self.start_alternate_pose[random_init][0] = }") + location = carla.Transform( + carla.Location( + x=self.start_alternate_pose[pose_init][0], + y=self.start_alternate_pose[pose_init][1], + z=self.start_alternate_pose[pose_init][2], + ), + carla.Rotation( + pitch=self.start_alternate_pose[pose_init][3], + yaw=self.start_alternate_pose[pose_init][4], + roll=self.start_alternate_pose[pose_init][5], + ), + ) + + """ these is for apply_batch_sync""" + # self.car = carla.command.SpawnActor(self.vehicle, location) + # while self.car is None: + # self.car = carla.command.SpawnActor(self.vehicle, location) + + # self.batch.append(self.car) + + """ original """ + self.car = self.world.spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(self.vehicle, location) + + # self.batch.append(self.car) + + self.actor_list.append(self.car) + + time.sleep(1) + + def setup_car_alternate_pose(self, init_positions): + random_init = np.random.randint(0, high=len(init_positions)) + # print(f"{random_init = }") + # print(f"{self.start_alternate_pose = }") + # print(f"{self.start_alternate_pose[random_init][0] = }") + location = carla.Transform( + carla.Location( + x=self.start_alternate_pose[random_init][0], + y=self.start_alternate_pose[random_init][1], + z=self.start_alternate_pose[random_init][2], + ), + carla.Rotation( + pitch=self.start_alternate_pose[random_init][3], + yaw=self.start_alternate_pose[random_init][4], + roll=self.start_alternate_pose[random_init][5], + ), + ) + + self.car = self.world.spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(self.vehicle, location) + + self.actor_list.append(self.car) + time.sleep(1) + + def setup_car_fix_pose(self, init): + """ + Town07, road 20, -1, init curves + (73.7, -10, 0.3, 0.0, -62.5, 0.0) + """ + + """ + # car_bp = self.world.get_blueprint_library().filter("vehicle.*")[0] + location = carla.Transform( + carla.Location( + x=init.transform.location.x, + y=init.transform.location.y, + z=init.transform.location.z, + ), + carla.Rotation( + pitch=init.transform.rotation.pitch, + yaw=init.transform.rotation.yaw, + roll=init.transform.rotation.roll, + ), + ) + + print(f"{init.transform.location.x = }") + print(f"{init.transform.location.y = }") + print(f"{init.lane_id = }") + print(f"{init.road_id = }") + print(f"{init.s = }") + print(f"{init.id = }") + print(f"{init.lane_width = }") + print(f"{init.lane_change = }") + print(f"{init.lane_type = }") + print(f"{init.right_lane_marking = }") + """ + + location = carla.Transform( + carla.Location(x=73.7, y=-10, z=0.300000), + carla.Rotation(pitch=0.000000, yaw=-62.5, roll=0.000000), + ) + + self.car = self.world.spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(self.vehicle, location) + + self.actor_list.append(self.car) + time.sleep(1) + + def setup_car_random_pose(self): + # car_bp = self.world.get_blueprint_library().filter("vehicle.*")[0] + location = random.choice(self.world.get_map().get_spawn_points()) + self.car = self.world.try_spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.try_spawn_actor(self.vehicle, location) + + # self.batch.append(self.car) + self.actor_list.append(self.car) + time.sleep(1) + + ################################################################## + # + # Sensors + ################################################################## + + ####################################################### + # + # Detectors + ####################################################### + + # @profile + def setup_col_sensor_weakref(self): + """weakref""" + # colsensor = self.world.get_blueprint_library().find("sensor.other.collision") + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.col_sensor = self.world.spawn_actor( + self.colsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.col_sensor) + weak_self = weakref.ref(self) + self.col_sensor.listen( + lambda event: FollowLaneDDPGStaticWeatherNoTraffic._collision_data( + weak_self, event + ) + ) + # self.col_sensor.listen(self.collision_data) + + @staticmethod + def _collision_data(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + + self.collision_hist.append(event) + actor_we_collide_against = event.other_actor + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + print( + f"you have crashed with {actor_we_collide_against.type_id} with impulse {impulse} and intensity {intensity}" + ) + + def setup_col_sensor(self): + # colsensor = self.world.get_blueprint_library().find("sensor.other.collision") + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.col_sensor = self.world.spawn_actor( + self.colsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.col_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + self.col_sensor.listen(self.collision_data) + + def collision_data(self, event): + self.collision_hist.append(event) + actor_we_collide_against = event.other_actor + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + print( + f"you have crashed with {actor_we_collide_against.type_id} with impulse {impulse} and intensity {intensity}" + ) + # self.actor_list.append(actor_we_collide_against) + + def setup_lane_invasion_sensor_weakref(self): + """weakref""" + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.lane_sensor = self.world.spawn_actor( + self.laneinvsensor, transform, attach_to=self.car + ) + + # self.lane_sensor = carla.command.SpawnActor( + # self.laneinvsensor, transform, parent=self.car + # ) + + # self.batch.append(self.lane_sensor) + self.actor_list.append(self.lane_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + weak_self = weakref.ref(self) + # self.lane_sensor.listen(self.lane_changing_data) + self.lane_sensor.listen( + lambda event: FollowLaneDDPGStaticWeatherNoTraffic._lane_changing( + weak_self, event + ) + ) + + @staticmethod + def _lane_changing(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + self.lane_changing_hist.append(event) + # print(f"you have changed the lane") + + def setup_lane_invasion_sensor(self): + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.lane_sensor = self.world.spawn_actor( + self.laneinvsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.lane_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + self.lane_sensor.listen(self.lane_changing_data) + + def lane_changing_data(self, event): + self.lane_changing_hist.append(event) + print(f"you have changed the lane") + + def setup_obstacle_sensor_weakref(self): + """weakref""" + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.obstacle_sensor = self.world.spawn_actor( + self.obstsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.obstacle_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + weak_self = weakref.ref(self) + # self.obstacle_sensor.listen(self.obstacle_data) + self.obstacle_sensor.listen( + lambda event: FollowLaneDDPGStaticWeatherNoTraffic._obstacle_sensor( + weak_self, event + ) + ) + + @staticmethod + def _obstacle_sensor(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + self.obstacle_hist.append(event) + print(f"you have found an obstacle") + + def setup_obstacle_sensor(self): + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.obstacle_sensor = self.world.spawn_actor( + self.obstsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.obstacle_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + self.obstacle_sensor.listen(self.obstacle_data) + + def obstacle_data(self, event): + self.obstacle_hist.append(event) + print(f"you have found an obstacle") + + ####################################### + # + # spectator + ####################################### + def setup_spectator(self): + # self.spectator = self.world.get_spectator() + car_transfor = self.car.get_transform() + # world_snapshot = self.world.wait_for_tick() + # actor_snapshot = world_snapshot.find(self.car.id) + # Set spectator at given transform (vehicle transform) + # self.spectator.set_transform(actor_snapshot.get_transform()) + self.spectator.set_transform( + carla.Transform( + car_transfor.location + carla.Location(z=60), + carla.Rotation(pitch=-90, roll=-90), + ) + ) + # self.actor_list.append(self.spectator) + + ###################################################### + # + # Cameras + ###################################################### + + ######################################## + # + # RGB + ######################################## + + # @profile + def setup_rgb_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_rgb = self.world.spawn_actor( + self.rgb_cam, transform, attach_to=self.car + ) + + # self.sensor_camera_rgb = carla.command.SpawnActor( + # self.rgb_cam, transform, parent=self.car + # ) + + # self.batch.append(self.sensor_camera_rgb) + + self.actor_list.append(self.sensor_camera_rgb) + # print(f"{len(self.actor_list) = }") + weak_self = weakref.ref(self) + # self.sensor_camera_rgb.listen(self.save_rgb_image) + self.sensor_camera_rgb.listen( + lambda event: FollowLaneDDPGStaticWeatherNoTraffic._rgb_image( + weak_self, event + ) + ) + + @staticmethod + def _rgb_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + + image = np.array(image.raw_data) + image = image.reshape((480, 640, 4)) + # image = image.reshape((512, 1024, 4)) + image = image[:, :, :3] + # self._data_dict["image"] = image3 + self.front_rgb_camera = image + + def setup_rgb_camera(self): + # print("enter setup_rg_camera") + # rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # rgb_cam.set_attribute("image_size_x", f"{self.width}") + # rgb_cam.set_attribute("image_size_y", f"{self.height}") + # rgb_cam.set_attribute("fov", f"110") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_rgb = self.world.spawn_actor( + self.rgb_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_rgb) + # print(f"{len(self.actor_list) = }") + self.sensor_camera_rgb.listen(self.save_rgb_image) + + def save_rgb_image(self, data: carla.Image): + if not isinstance(data, carla.Image): + raise ValueError("Argument must be a carla.Image") + image = np.array(data.raw_data) + image = image.reshape((480, 640, 4)) + image = image[:, :, :3] + # self._data_dict["image"] = image3 + self.front_rgb_camera = image + + def _save_rgb_image(self, image): + # t_start = self.timer.time() + + image.convert(carla.ColorConverter.Raw) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + # if self.display_man.render_enabled(): + # self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + # cv2.imshow("", array) + # cv2.waitKey(1) + self.front_rgb_camera = array + + ################## + # + # Red Mask Camera + ################## + def setup_red_mask_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_red_mask = self.world.spawn_actor( + self.red_mask_cam, transform, attach_to=self.car + ) + + # self.sensor_camera_red_mask = carla.command.SpawnActor( + # self.red_mask_cam, transform, parent=self.car + # ) + + # self.batch.append(self.sensor_camera_red_mask) + self.actor_list.append(self.sensor_camera_red_mask) + weak_self = weakref.ref(self) + + # start_camera_red_mask = time.time() + self.sensor_camera_red_mask.listen( + lambda event: FollowLaneDDPGStaticWeatherNoTraffic._red_mask_semantic_image( + weak_self, event + ) + ) + # end_camera_red_mask = time.time() + # print( + # f"\n====> sensor_camera_red_mask time processing: {end_camera_red_mask - start_camera_red_mask = }" + # ) + + # self.sensor_camera_red_mask.listen(self.save_red_mask_semantic_image) + + @staticmethod + def _red_mask_semantic_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + hsv_nemo = cv2.cvtColor(array, cv2.COLOR_RGB2HSV) + + if ( + self.world.get_map().name == "Carla/Maps/Town07" + or self.world.get_map().name == "Carla/Maps/Town04" + or self.world.get_map().name == "Carla/Maps/Town07_Opt" + or self.world.get_map().name == "Carla/Maps/Town04_Opt" + ): + light_sidewalk = (42, 200, 233) + dark_sidewalk = (44, 202, 235) + else: + light_sidewalk = (151, 217, 243) + dark_sidewalk = (153, 219, 245) + + light_pavement = (149, 127, 127) + dark_pavement = (151, 129, 129) + + mask_sidewalk = cv2.inRange(hsv_nemo, light_sidewalk, dark_sidewalk) + # result_sidewalk = cv2.bitwise_and(array, array, mask=mask_sidewalk) + + mask_pavement = cv2.inRange(hsv_nemo, light_pavement, dark_pavement) + # result_pavement = cv2.bitwise_and(array, array, mask=mask_pavement) + + # Adjust according to your adjacency requirement. + kernel = np.ones((3, 3), dtype=np.uint8) + + # Dilating masks to expand boundary. + color1_mask = cv2.dilate(mask_sidewalk, kernel, iterations=1) + color2_mask = cv2.dilate(mask_pavement, kernel, iterations=1) + + # Required points now will have both color's mask val as 255. + common = cv2.bitwise_and(color1_mask, color2_mask) + SOME_THRESHOLD = 0 + + # Common is binary np.uint8 image, min = 0, max = 255. + # SOME_THRESHOLD can be anything within the above range. (not needed though) + # Extract/Use it in whatever way you want it. + intersection_points = np.where(common > SOME_THRESHOLD) + + # Say you want these points in a list form, then you can do this. + pts_list = [[r, c] for r, c in zip(*intersection_points)] + # print(pts_list) + + # for x, y in pts_list: + # image_2[x][y] = (255, 0, 0) + + # red_line_mask = np.zeros((400, 500, 3), dtype=np.uint8) + red_line_mask = np.zeros((image.height, image.width, 3), dtype=np.uint8) + + for x, y in pts_list: + red_line_mask[x][y] = (255, 0, 0) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + red_line_mask = cv2.cvtColor(red_line_mask, cv2.COLOR_BGR2RGB) + self.front_red_mask_camera = red_line_mask + + def setup_red_mask_camera(self): + # self.red_mask_cam = self.world.get_blueprint_library().find( + # "sensor.camera.semantic_segmentation" + # ) + # self.red_mask_cam.set_attribute("image_size_x", f"{self.width}") + # self.red_mask_cam.set_attribute("image_size_y", f"{self.height}") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_red_mask = self.world.spawn_actor( + self.red_mask_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_red_mask) + self.sensor_camera_red_mask.listen(self.save_red_mask_semantic_image) + + def save_red_mask_semantic_image(self, image): + # t_start = self.timer.time() + # print(f"en red_mask calbback") + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + hsv_nemo = cv2.cvtColor(array, cv2.COLOR_RGB2HSV) + + if ( + self.world.get_map().name == "Carla/Maps/Town07" + or self.world.get_map().name == "Carla/Maps/Town04" + ): + light_sidewalk = (42, 200, 233) + dark_sidewalk = (44, 202, 235) + else: + light_sidewalk = (151, 217, 243) + dark_sidewalk = (153, 219, 245) + + light_pavement = (149, 127, 127) + dark_pavement = (151, 129, 129) + + mask_sidewalk = cv2.inRange(hsv_nemo, light_sidewalk, dark_sidewalk) + # result_sidewalk = cv2.bitwise_and(array, array, mask=mask_sidewalk) + + mask_pavement = cv2.inRange(hsv_nemo, light_pavement, dark_pavement) + # result_pavement = cv2.bitwise_and(array, array, mask=mask_pavement) + + # Adjust according to your adjacency requirement. + kernel = np.ones((3, 3), dtype=np.uint8) + + # Dilating masks to expand boundary. + color1_mask = cv2.dilate(mask_sidewalk, kernel, iterations=1) + color2_mask = cv2.dilate(mask_pavement, kernel, iterations=1) + + # Required points now will have both color's mask val as 255. + common = cv2.bitwise_and(color1_mask, color2_mask) + SOME_THRESHOLD = 0 + + # Common is binary np.uint8 image, min = 0, max = 255. + # SOME_THRESHOLD can be anything within the above range. (not needed though) + # Extract/Use it in whatever way you want it. + intersection_points = np.where(common > SOME_THRESHOLD) + + # Say you want these points in a list form, then you can do this. + pts_list = [[r, c] for r, c in zip(*intersection_points)] + # print(pts_list) + + # for x, y in pts_list: + # image_2[x][y] = (255, 0, 0) + + # red_line_mask = np.zeros((400, 500, 3), dtype=np.uint8) + red_line_mask = np.zeros((image.height, image.width, 3), dtype=np.uint8) + + for x, y in pts_list: + red_line_mask[x][y] = (255, 0, 0) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + red_line_mask = cv2.cvtColor(red_line_mask, cv2.COLOR_BGR2RGB) + self.front_red_mask_camera = red_line_mask + + ################## + # + # BEV + ################## + def setup_bev_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_bev_camera = self.world.spawn_actor( + self.bev_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_bev_camera) + # print(f"{len(self.actor_list) = }") + weak_self = weakref.ref(self) + # self.sensor_bev_camera.listen(self.save_bev_image) + self.sensor_bev_camera.listen( + lambda event: FollowLaneDDPGStaticWeatherNoTraffic._bev_image( + weak_self, event + ) + ) + + @staticmethod + def _bev_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + + car_bp = self.world.get_actors().filter("vehicle.*")[0] + # car_bp = self.vehicle + birdview = self.birdview_producer.produce( + agent_vehicle=car_bp # carla.Actor (spawned vehicle) + ) + image = BirdViewProducer.as_rgb(birdview) + hsv_nemo = cv2.cvtColor(image, cv2.COLOR_RGB2HSV) + + # Extract central line + light_center_line = (17, 134, 232) + dark_center_line = (19, 136, 234) + mask_center_line = cv2.inRange(hsv_nemo, light_center_line, dark_center_line) + result = cv2.bitwise_and(hsv_nemo, hsv_nemo, mask=mask_center_line) + + # image = np.rot90(image) + image = np.array(image) + result = np.array(result) + if image.shape[0] != image.shape[1]: + if image.shape[0] > image.shape[1]: + difference = image.shape[0] - image.shape[1] + extra_left, extra_right = int(difference / 2), int(difference / 2) + extra_top, extra_bottom = 0, 0 + else: + difference = image.shape[1] - image.shape[0] + extra_left, extra_right = 0, 0 + extra_top, extra_bottom = int(difference / 2), int(difference / 2) + image = np.pad( + image, + ((extra_top, extra_bottom), (extra_left, extra_right), (0, 0)), + mode="constant", + constant_values=0, + ) + image = np.pad( + image, + ((100, 100), (50, 50), (0, 0)), + mode="constant", + constant_values=0, + ) + + self.front_camera_bev = image + self.front_camera_bev_mask = result + + def setup_bev_camera(self): + # print("enter setup_rg_camera") + # rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # rgb_cam.set_attribute("image_size_x", f"{self.width}") + # rgb_cam.set_attribute("image_size_y", f"{self.height}") + # rgb_cam.set_attribute("fov", f"110") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_bev_camera = self.world.spawn_actor( + self.bev_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_bev_camera) + # print(f"{len(self.actor_list) = }") + self.sensor_bev_camera.listen(self.save_bev_image) + + def save_bev_image(self, image): + car_bp = self.world.get_actors().filter("vehicle.*")[0] + # car_bp = self.vehicle + birdview = self.birdview_producer.produce( + agent_vehicle=car_bp # carla.Actor (spawned vehicle) + ) + image = BirdViewProducer.as_rgb(birdview) + hsv_nemo = cv2.cvtColor(image, cv2.COLOR_RGB2HSV) + + # Extract central line + light_center_line = (17, 134, 232) + dark_center_line = (19, 136, 234) + mask_center_line = cv2.inRange(hsv_nemo, light_center_line, dark_center_line) + result = cv2.bitwise_and(hsv_nemo, hsv_nemo, mask=mask_center_line) + + # image = np.rot90(image) + image = np.array(image) + result = np.array(result) + if image.shape[0] != image.shape[1]: + if image.shape[0] > image.shape[1]: + difference = image.shape[0] - image.shape[1] + extra_left, extra_right = int(difference / 2), int(difference / 2) + extra_top, extra_bottom = 0, 0 + else: + difference = image.shape[1] - image.shape[0] + extra_left, extra_right = 0, 0 + extra_top, extra_bottom = int(difference / 2), int(difference / 2) + image = np.pad( + image, + ((extra_top, extra_bottom), (extra_left, extra_right), (0, 0)), + mode="constant", + constant_values=0, + ) + image = np.pad( + image, + ((100, 100), (50, 50), (0, 0)), + mode="constant", + constant_values=0, + ) + + self.front_camera_bev = image + self.front_camera_bev_mask = result + + ################## + # + # Segmentation Camera + ################## + + def setup_segmentation_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_segmentation = self.world.spawn_actor( + self.segm_cam, transform, attach_to=self.car + ) + + # self.sensor_camera_segmentation = carla.command.SpawnActor( + # self.segm_cam, transform, parent=self.car + # ) + + # self.batch.append(self.sensor_camera_segmentation) + self.actor_list.append(self.sensor_camera_segmentation) + # print(f"{len(self.actor_list) = }") + weak_self = weakref.ref(self) + # self.sensor_camera_segmentation.listen(self.save_segmentation_image) + self.sensor_camera_segmentation.listen( + lambda event: FollowLaneDDPGStaticWeatherNoTraffic._segmentation_image( + weak_self, event + ) + ) + + @staticmethod + def _segmentation_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + self.segmentation_cam = array + + def setup_segmentation_camera(self): + # print("enter setup_rg_camera") + # rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # rgb_cam.set_attribute("image_size_x", f"{self.width}") + # rgb_cam.set_attribute("image_size_y", f"{self.height}") + # rgb_cam.set_attribute("fov", f"110") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_segmentation = self.world.spawn_actor( + self.segm_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_segmentation) + # print(f"{len(self.actor_list) = }") + self.sensor_camera_segmentation.listen(self.save_segmentation_image) + + def save_segmentation_image(self, image: carla.Image): + # t_start = self.timer.time() + + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + self.segmentation_cam = array + # if self.display_man.render_enabled(): + # self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + ################## + # + # Destroyers + ################## + + def destroy_all_actors_apply_batch(self): + print("\ndestroying %d actors with apply_batch method" % len(self.actor_list)) + self.client.apply_batch( + [carla.command.DestroyActor(x) for x in self.actor_list[::-1]] + ) + + def destroy_all_actors(self): + print("\ndestroying %d actors with destroy() method" % len(self.actor_list)) + for actor in self.actor_list[::-1]: + actor.destroy() + + #################################################################### + # + # Step + # + #################################################################### + + def step(self, action): + # from rl_studio.envs.gazebo.f1.models.step import ( + # StepFollowLane, + # ) + + if ( + self.state_space == "image" + and self.action_space != "continuous" + and self.states_entry == "only_right_line" + ): + return self.step_followlane_state_image_actions_discretes_only_right_line( + action + ) + # Faltan 2 mas por crear + + elif ( + self.state_space == "image" + and self.action_space == "continuous" + and self.states_entry == "only_right_line" + ): + """ + state: image + actions: continuous + mark: only right line + """ + + return self.step_followlane_state_image_actions_continuous_only_right_line( + action + ) + elif ( + self.state_space == "image" + and self.action_space == "continuous" + and self.states_entry == "between_right_left_lines" + ): + """ + state: image + actions: continuous + mark: between_right_left_lines (for left and right continuous lines) + """ + return self.step_followlane_state_image_actions_continuous_between_right_left_lines( + action + ) + + elif ( + self.state_space == "image" + and self.action_space == "continuous" + and self.states_entry == "lane_detector_pytorch" + ): + """ + state: image + actions: continuous + mark: pythorch lane detector + """ + return self.step_followlane_state_image_actions_continuous_lane_detector_pytorch( + action + ) + + elif ( + self.state_space != "image" + and self.action_space == "continuous" + and self.states_entry == "only_right_line" + ): + self.step_followlane_state_sp_actions_continuous_only_right_line(action) + else: # aqui faltaria el state_sp_actions_discretes_lane_detector, pero antes hay qye crear los otros 2 + self.step_followlane_state_sp_actions_discretes(action) + + ######################################################## + # step + # state: image + # actions: continuous + # only right line + ####################################################### + def step_followlane_state_image_actions_continuous_only_right_line(self, action): + """ + state: Image + actions: continuous + only right line + """ + + self.control_only_right(action) + + ########### --- calculating STATES + # start_camera_red_mask = time.time() + mask = self.simplifiedperception.preprocess_image(self.front_red_mask_camera) + + ########### --- Calculating center ONLY with right line + right_line_in_pixels = self.simplifiedperception.calculate_right_line( + mask, self.x_row + ) + + image_center = mask.shape[1] // 2 + dist = [ + image_center - right_line_in_pixels[i] + for i, _ in enumerate(right_line_in_pixels) + ] + dist_normalized = [ + float((image_center - right_line_in_pixels[i]) / image_center) + for i, _ in enumerate(right_line_in_pixels) + ] + + pixels_in_state = mask.shape[1] // self.num_regions + state_right_lines = [ + i for i in range(1, mask.shape[1]) if i % pixels_in_state == 0 + ] + + ## STATES + + # states = [ + # int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + # ] + # states = np.array(self.convert_segment_image_to_mask(self.segmentation_cam)) + states = np.array( + self.simplifiedperception.resize_and_trimming_right_line_mask(mask) + ) + + # print(f"{lane_centers_in_pixels = }") + + ## -------- Ending Step()... + done = False + ground_truth_normal_values = [ + correct_normalized_distance[value] for i, value in enumerate(self.x_row) + ] + # reward, done = self.autocarlarewards.rewards_right_line( + # dist_normalized, ground_truth_normal_values, self.params + # ) + reward, done = self.autocarlarewards.rewards_sigmoid_only_right_line( + dist_normalized, ground_truth_normal_values + ) + + ## -------- ... or Finish by... + if len(self.collision_hist) > 0: # crashed you, baby + done = True + # reward = -100 + print(f"crash") + + self.is_finish, self.dist_to_finish = AutoCarlaUtils.finish_fix_number_target( + self.params["location"], + self.finish_alternate_pose, + self.finish_pose_number, + self.max_target_waypoint_distance, + ) + if self.is_finish: + print(f"Finish!!!!") + done = True + # reward = 100 + + # if len(self.lane_changing_hist) > 1: # you leave the lane + # done = True + # reward = -100 + # print(f"out of lane") + + ground_truth_pixel_values = [ + correct_pixel_distance[value] for i, value in enumerate(self.x_row) + ] + + AutoCarlaUtils.show_image( + "states", + states, + 600, + 400, + ) + + # AutoCarlaUtils.show_image( + # "segmentation_cam", + # self.segmentation_cam, + # 1200, + # 400, + # ) + + # AutoCarlaUtils.show_image( + # "self.sensor_camera_segmentation", + # self.sensor_camera_segmentation, + # 1200, + # 1000, + # ) + + AutoCarlaUtils.show_image_only_right_line( + "mask", + mask, + 1, + right_line_in_pixels, + ground_truth_pixel_values, + dist_normalized, + [4, 4, 4, 4], + self.x_row, + 600, + 10, + state_right_lines, + ) + + AutoCarlaUtils.show_image_only_right_line( + "front RGB", + self.front_rgb_camera[(self.front_rgb_camera.shape[0] // 2) :], + 1, + right_line_in_pixels, + ground_truth_pixel_values, + dist_normalized, + [4, 4, 4, 4], + self.x_row, + 1250, + 10, + state_right_lines, + ) + + render_params( + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + steering_angle=self.params["steering_angle"], + # Steering_angle=self.params["Steering_angle"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + height=self.params["height"], + action=action, + speed_kmh=self.params["speed"], + acceleration_ms2=self.params["Acceleration"], + _="------------------------", + # states=states, + # lane_centers_in_pixels=lane_centers_in_pixels, + # image_center=image_center, + right_line_in_pixels=right_line_in_pixels, + dist=dist, + dist_normalized=dist_normalized, + ground_truth_normal_values=ground_truth_normal_values, + reward=reward, + done=done, + distance_to_finish=self.dist_to_finish, + num_self_lane_change_hist=len(self.lane_changing_hist), + num_self_collision_hist=len(self.collision_hist), + num_self_obstacle_hist=len(self.obstacle_hist), + ) + """ + print_messages( + "in step()", + height=mask.shape[0], + width=mask.shape[1], + action=action, + velocity=self.params["speed"], + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + _="------------------------", + states=states, + right_line_in_pixels=right_line_in_pixels, + dist=dist, + dist_normalized=dist_normalized, + # self_perfect_distance_pixels=self.perfect_distance_pixels, + # self_perfect_distance_normalized=self.perfect_distance_normalized, + # error=error, + done=done, + reward=reward, + distance_to_finish=self.dist_to_finish, + self_collision_hist=self.collision_hist, + num_self_obstacle_hist=len(self.obstacle_hist), + num_self_lane_change_hist=len(self.lane_changing_hist), + ) + """ + return states, reward, done, {} + + ######################################################## + # step + # state: image + # actions: continuous + # only right line + ####################################################### + def step_followlane_state_sp_actions_continuous_only_right_line(self, action): + """ + state: sp + actions: continuous + only right line + """ + + self.control_only_right(action) + + ########### --- calculating STATES + # start_camera_red_mask = time.time() + mask = self.simplifiedperception.preprocess_image(self.front_red_mask_camera) + + ########### --- Calculating center ONLY with right line + right_line_in_pixels = self.simplifiedperception.calculate_right_line( + mask, self.x_row + ) + + image_center = mask.shape[1] // 2 + dist = [ + image_center - right_line_in_pixels[i] + for i, _ in enumerate(right_line_in_pixels) + ] + dist_normalized = [ + float((image_center - right_line_in_pixels[i]) / image_center) + for i, _ in enumerate(right_line_in_pixels) + ] + + pixels_in_state = mask.shape[1] // self.num_regions + state_right_lines = [ + i for i in range(1, mask.shape[1]) if i % pixels_in_state == 0 + ] + + ## STATES + + states = [ + int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + ] + + ## -------- Ending Step()... + done = False + ground_truth_normal_values = [ + correct_normalized_distance[value] for i, value in enumerate(self.x_row) + ] + # reward, done = self.autocarlarewards.rewards_right_line( + # dist_normalized, ground_truth_normal_values, self.params + # ) + reward, done = self.autocarlarewards.rewards_sigmoid_only_right_line( + dist_normalized, ground_truth_normal_values + ) + + ## -------- ... or Finish by... + if len(self.collision_hist) > 0: # crashed you, baby + done = True + # reward = -100 + print(f"crash") + + self.is_finish, self.dist_to_finish = AutoCarlaUtils.finish_fix_number_target( + self.params["location"], + self.finish_alternate_pose, + self.finish_pose_number, + self.max_target_waypoint_distance, + ) + if self.is_finish: + print(f"Finish!!!!") + done = True + # reward = 100 + + # if len(self.lane_changing_hist) > 1: # you leave the lane + # done = True + # reward = -100 + # print(f"out of lane") + + ground_truth_pixel_values = [ + correct_pixel_distance[value] for i, value in enumerate(self.x_row) + ] + + AutoCarlaUtils.show_image( + "states", + states, + 600, + 400, + ) + + # AutoCarlaUtils.show_image( + # "segmentation_cam", + # self.segmentation_cam, + # 1200, + # 400, + # ) + + # AutoCarlaUtils.show_image( + # "self.sensor_camera_segmentation", + # self.sensor_camera_segmentation, + # 1200, + # 1000, + # ) + + AutoCarlaUtils.show_image_only_right_line( + "mask", + mask, + 1, + right_line_in_pixels, + ground_truth_pixel_values, + dist_normalized, + [4, 4, 4, 4], + self.x_row, + 600, + 10, + state_right_lines, + ) + + AutoCarlaUtils.show_image_only_right_line( + "front RGB", + self.front_rgb_camera[(self.front_rgb_camera.shape[0] // 2) :], + 1, + right_line_in_pixels, + ground_truth_pixel_values, + dist_normalized, + [4, 4, 4, 4], + self.x_row, + 1250, + 10, + state_right_lines, + ) + + render_params( + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + steering_angle=self.params["steering_angle"], + # Steering_angle=self.params["Steering_angle"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + height=self.params["height"], + action=action, + speed_kmh=self.params["speed"], + acceleration_ms2=self.params["Acceleration"], + _="------------------------", + # states=states, + # lane_centers_in_pixels=lane_centers_in_pixels, + # image_center=image_center, + right_line_in_pixels=right_line_in_pixels, + dist=dist, + dist_normalized=dist_normalized, + ground_truth_normal_values=ground_truth_normal_values, + reward=reward, + done=done, + distance_to_finish=self.dist_to_finish, + num_self_lane_change_hist=len(self.lane_changing_hist), + num_self_collision_hist=len(self.collision_hist), + num_self_obstacle_hist=len(self.obstacle_hist), + ) + """ + print_messages( + "in step()", + height=mask.shape[0], + width=mask.shape[1], + action=action, + velocity=self.params["speed"], + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + _="------------------------", + states=states, + right_line_in_pixels=right_line_in_pixels, + dist=dist, + dist_normalized=dist_normalized, + # self_perfect_distance_pixels=self.perfect_distance_pixels, + # self_perfect_distance_normalized=self.perfect_distance_normalized, + # error=error, + done=done, + reward=reward, + distance_to_finish=self.dist_to_finish, + self_collision_hist=self.collision_hist, + num_self_obstacle_hist=len(self.obstacle_hist), + num_self_lane_change_hist=len(self.lane_changing_hist), + ) + """ + return states, reward, done, {} + + def control_only_right(self, action): + """ + DDPG with continuos actions + """ + + t = self.car.get_transform() + v = self.car.get_velocity() + c = self.car.get_control() + w = self.car.get_angular_velocity() + a = self.car.get_acceleration() + self.params["speed"] = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + self.params["steering_angle"] = w + # self.params["Steering_angle"] = steering_angle + self.params["Steer"] = c.steer + self.params["location"] = (t.location.x, t.location.y) + self.params["Throttle"] = c.throttle + self.params["Brake"] = c.brake + self.params["height"] = t.location.z + self.params["Acceleration"] = math.sqrt(a.x**2 + a.y**2 + a.z**2) + + ## Applied throttle, brake and steer + curr_speed = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + target_speed = 30 + # throttle_low_limit = 0.0 #0.1 for low speeds + # vel_error = curr_speed / target_speed - 1 + # limit = 0.0 + # if vel_error > limit: + # brake = 1 + # throttle = 0.45 + # else: + # throttle = 0.7 + + brake = 0 + throttle = action[0][0] + steer = action[0][1] + + if curr_speed > target_speed: + throttle = 0.45 + # else: + # throttle = 0.8 + + # clip_throttle = self.clip_throttle( + # throttle_upper_limit, curr_speed, target_speed, throttle_low_limit + # ) + + # self.car.apply_control( + # carla.VehicleControl(throttle=clip_throttle, steer=steer) + # ) + + self.car.apply_control( + carla.VehicleControl(throttle=throttle, steer=steer, brake=brake) + ) + + ######################################################## + # utils + # + ######################################################## + + def __del__(self): + print("__del__ called") + + def checking_carla_server(self, town): + # print(f"checking Carla Server...") + + try: + ps_output = ( + subprocess.check_output(["ps", "-Af"]).decode("utf-8").strip("\n") + ) + except subprocess.CalledProcessError as ce: + print("SimulatorEnv: exception raised executing ps command {}".format(ce)) + sys.exit(-1) + + if ( + (ps_output.count("CarlaUE4.sh") == 0) + or (ps_output.count("CarlaUE4-Linux-Shipping") == 0) + or (ps_output.count("CarlaUE4-Linux-") == 0) + ): + try: + carla_root = os.environ["CARLA_ROOT"] + # print(f"{carla_root = }\n") + carla_exec = f"{carla_root}/CarlaUE4.sh" + + with open("/tmp/.carlalaunch_stdout.log", "w") as out, open( + "/tmp/.carlalaunch_stderr.log", "w" + ) as err: + print_messages( + "launching Carla Server again...", + ) + subprocess.Popen( + [carla_exec, "-prefernvidia"], stdout=out, stderr=err + ) + time.sleep(5) + self.world = self.client.load_world(town) + + except subprocess.CalledProcessError as ce: + print( + "SimulatorEnv: exception raised executing killall command for CARLA server {}".format( + ce + ) + ) diff --git a/rl_studio/envs/carla/followlane/followlane_dqn.py b/rl_studio/envs/carla/followlane/followlane_dqn.py new file mode 100644 index 000000000..f5e873f0c --- /dev/null +++ b/rl_studio/envs/carla/followlane/followlane_dqn.py @@ -0,0 +1,1867 @@ +from collections import Counter +import copy +from datetime import datetime, timedelta +import math +import os +import subprocess +import time +import sys + +import carla +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions +from cv_bridge import CvBridge +import cv2 +from geometry_msgs.msg import Twist +import matplotlib.pyplot as plt +from memory_profiler import profile +import numpy as np +import pygame +import random +import weakref +import rospy +from sensor_msgs.msg import Image +import torch +import torchvision + +from rl_studio.agents.utils import ( + print_messages, + render_params, +) +from rl_studio.envs.carla.followlane.followlane_env import FollowLaneEnv +from rl_studio.envs.carla.followlane.settings import FollowLaneCarlaConfig +from rl_studio.envs.carla.followlane.utils import AutoCarlaUtils + +from rl_studio.envs.carla.utils.bounding_boxes import BasicSynchronousClient +from rl_studio.envs.carla.utils.visualize_multiple_sensors import ( + DisplayManager, + SensorManager, + CustomTimer, +) + +from rl_studio.envs.carla.utils.global_route_planner import ( + GlobalRoutePlanner, +) + +correct_normalized_distance = { + 20: -0.07, + 30: -0.1, + 40: -0.13, + 50: -0.17, + 60: -0.2, + 70: -0.23, + 80: -0.26, + 90: -0.3, + 100: -0.33, + 110: -0.36, + 120: -0.4, + 130: -0.42, + 140: -0.46, + 150: -0.49, + 160: -0.52, + 170: -0.56, + 180: -0.59, + 190: -0.62, + 200: -0.65, + 210: -0.69, + 220: -0.72, +} +correct_pixel_distance = { + 20: 343, + 30: 353, + 40: 363, + 50: 374, + 60: 384, + 70: 394, + 80: 404, + 90: 415, + 100: 425, + 110: 436, + 120: 446, + 130: 456, + 140: 467, + 150: 477, + 160: 488, + 170: 498, + 180: 508, + 190: 518, + 200: 528, + 210: 540, + 220: 550, +} + + +class FollowLaneDQNStaticWeatherNoTraffic(FollowLaneEnv): + def __init__(self, **config): + ## --------------- init env + FollowLaneEnv.__init__(self, **config) + ## --------------- init class variables + FollowLaneCarlaConfig.__init__(self, **config) + + self.timer = CustomTimer() + + self.client = carla.Client( + config["carla_server"], + config["carla_client"], + ) + self.client.set_timeout(3.0) + + # print(f"\n entre en DQN\n") + print(f"\n maps in carla 0.9.13: {self.client.get_available_maps()}\n") + + """ + self.world = self.client.get_world() + self.world = self.client.load_world(config["town"]) + settings = self.world.get_settings() + settings.fixed_delta_seconds = 0.05 + if config["sync"]: + settings.synchronous_mode = True + else: + settings.synchronous_mode = False + self.world.apply_settings(settings) + + """ + + self.world = self.client.load_world(config["town"]) + if config["town"] == "Town07_Opt": + self.world.unload_map_layer(carla.MapLayer.Buildings) + self.world.unload_map_layer(carla.MapLayer.Decals) + self.world.unload_map_layer(carla.MapLayer.Foliage) + self.world.unload_map_layer(carla.MapLayer.Particles) + self.world.unload_map_layer(carla.MapLayer.Props) + + self.original_settings = self.world.get_settings() + + # TODO: si algo se jode hay que quitar esta linea + self.traffic_manager = self.client.get_trafficmanager(8000) + settings = self.world.get_settings() + settings.fixed_delta_seconds = 0.1 # 0.05 + if config["sync"]: + # TODO: si algo se jode hay que quitar esta linea + self.traffic_manager.set_synchronous_mode(True) + # settings.synchronous_mode = True ####OJOOO + else: + self.traffic_manager.set_synchronous_mode(False) + # settings.synchronous_mode = False ###OJOJOOO + + self.world.apply_settings(settings) + + ### Weather + weather = carla.WeatherParameters( + cloudiness=70.0, precipitation=0.0, sun_altitude_angle=70.0 + ) + self.world.set_weather(weather) + + """este no funciona + self.world = self.client.load_world(config["town"]) + # self.original_settings = self.world.get_settings() + # self.traffic_manager = self.client.get_trafficmanager(8000) + settings = self.world.get_settings() + settings.fixed_delta_seconds = 0.05 + if config["sync"]: + settings.synchronous_mode = True + else: + settings.synchronous_mode = False + self.world.apply_settings(settings) + """ + + ## -- display manager + # self.display_manager = DisplayManager( + # grid_size=[2, 2], + # window_size=[900, 600], + # ) + + ## --------------- Blueprint --------------- + self.blueprint_library = self.world.get_blueprint_library() + ## --------------- Car --------------- + self.vehicle = self.world.get_blueprint_library().filter("vehicle.*")[0] + self.car = None + ## --------------- collision sensor --------------- + self.colsensor = self.world.get_blueprint_library().find( + "sensor.other.collision" + ) + self.collision_hist = [] + self.col_sensor = None + ## --------------- Lane invasion sensor --------------- + self.laneinvsensor = self.world.get_blueprint_library().find( + "sensor.other.lane_invasion" + ) + self.lane_changing_hist = [] + self.lane_sensor = None + ## --------------- Obstacle sensor --------------- + self.obstsensor = self.world.get_blueprint_library().find( + "sensor.other.obstacle" + ) + self.obstacle_hist = [] + self.obstacle_sensor = None + ## --------------- RGB camera --------------- + self.rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + self.rgb_cam.set_attribute("image_size_x", f"{self.width}") + self.rgb_cam.set_attribute("image_size_y", f"{self.height}") + self.rgb_cam.set_attribute("fov", f"110") + self.sensor_camera_rgb = None + self.front_rgb_camera = None + ## --------------- RedMask Camera --------------- + self.red_mask_cam = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + self.red_mask_cam.set_attribute("image_size_x", f"{self.width}") + self.red_mask_cam.set_attribute("image_size_y", f"{self.height}") + self.red_mask_cam.set_attribute("fov", f"110") + self.sensor_camera_red_mask = None + self.front_red_mask_camera = None + ## --------------- BEV camera --------------- + """ + self.birdview_producer = BirdViewProducer( + self.client, # carla.Client + target_size=PixelDimensions(width=100, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AREA_ONLY, + ) + self.bev_cam = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + self.bev_cam.set_attribute("image_size_x", f"{self.width}") + self.bev_cam.set_attribute("image_size_y", f"{self.height}") + self.front_camera_bev = None + self.front_camera_bev_mask = None + """ + ## --------------- Segmentation Camera --------------- + self.segm_cam = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + self.segm_cam.set_attribute("image_size_x", f"{self.width}") + self.segm_cam.set_attribute("image_size_y", f"{self.height}") + self.sensor_camera_segmentation = None + self.segmentation_cam = None + + ## --------------- LaneDetector Camera --------------- + # self.lanedetector_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # self.lanedetector_cam.set_attribute("image_size_x", f"{self.width}") + # self.lanedetector_cam.set_attribute("image_size_y", f"{self.height}") + # self.lanedetector_cam.set_attribute("fov", f"110") + # self.sensor_camera_lanedetector = None + # self.front_lanedetector_camera = None + + # self._detector = LaneDetector("models/fastai_torch_lane_detector_model.pth") + # torch.cuda.empty_cache() + # self.model = torch.load("models/fastai_torch_lane_detector_model.pth") + # self.model.eval() + + ## --------------- more --------------- + self.perfect_distance_pixels = None + self.perfect_distance_normalized = None + # self._control = carla.VehicleControl() + self.params = {} + self.target_waypoint = None + + self.spectator = self.world.get_spectator() + # self.spectator = None + self.actor_list = [] + self.is_finish = None + self.dist_to_finish = None + self.batch = [] + + ##################################################################################### + # + # RESET + # + ##################################################################################### + + def reset(self): + if self.state_space == "image": + return self.reset_image() + else: + return self.reset_sp() + + def reset_image(self): + """ + image + + """ + + if self.sensor_camera_rgb is not None: + self.destroy_all_actors_apply_batch() + + self.col_sensor = None + self.sensor_camera_rgb = None + self.sensor_camera_red_mask = None + self.front_camera_bev = None + self.front_camera_bev_mask = None + self.sensor_camera_segmentation = None + self.sensor_camera_lanedetector = None + + self.lane_sensor = None + self.obstacle_sensor = None + self.collision_hist = [] + self.lane_changing_hist = [] + self.obstacle_hist = [] + self.actor_list = [] + + ## --- Car + waypoints_town = self.world.get_map().generate_waypoints(5.0) + init_waypoint = waypoints_town[self.waypoints_init + 1] + filtered_waypoints = self.draw_waypoints( + waypoints_town, + self.waypoints_init, + self.waypoints_target, + self.waypoints_lane_id, + 2000, + ) + self.target_waypoint = filtered_waypoints[-1] + + ## ---- random init position in the whole Town: actually for test functioning porposes + if self.random_pose: + self.setup_car_random_pose() + ## -- Always same init position in a circuit predefined + elif self.alternate_pose is False: + self.setup_car_pose(self.start_alternate_pose, init=self.init_pose_number) + + ## -- Same circuit, but random init positions + else: + # self.setup_car_alternate_pose(self.start_alternate_pose) + self.setup_car_pose(self.start_alternate_pose) + + ## --- Cameras + + ## ---- RGB + # self.setup_rgb_camera() + self.setup_rgb_camera_weakref() + # self.setup_semantic_camera() + while self.sensor_camera_rgb is None: + time.sleep(0.01) + + ## ---- Red Mask + self.setup_red_mask_camera_weakref() + while self.sensor_camera_red_mask is None: + time.sleep(0.01) + + ## --- SEgmentation camera + self.setup_segmentation_camera_weakref() + # self.setup_segmentation_camera() + while self.sensor_camera_segmentation is None: + time.sleep(0.01) + + # ## ---- LAneDetector + # self.setup_lane_detector_camera_weakref() + # while self.sensor_camera_lane_detector is None: + # time.sleep(0.01) + + ## --- Detectors Sensors + self.setup_lane_invasion_sensor_weakref() + + """ apply_batch_sync for pedestrians and many cars """ + ## --- apply_batch_sync + # for response in self.client.apply_batch_sync(self.batch, True): + # if response.error: + # print(f"{response.error = }") + # logging.error(response.error) + # else: + # self.actor_list.append(response.actor_id) + + # NO results = client.apply_batch_sync(self.batch, True) + # for i in range(len(results)): + # if results[i].error: + # logging.error(results[i].error) + # else: + # self.actor_list[i]["con"] = results[i].actor_id + + time.sleep(1) + + ## Autopilot + # self.car.set_autopilot(True) + self.car.apply_control(carla.VehicleControl(throttle=0.0, brake=0.0)) + # self.setup_spectator() + + ########### --- calculating STATES + mask = self.simplifiedperception.preprocess_image(self.front_red_mask_camera) + # right_line_in_pixels = self.simplifiedperception.calculate_right_line( + # mask, self.x_row + # ) + + # pixels_in_state = mask.shape[1] / self.num_regions + # states = [ + # int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + # ] + + # states = np.array(self.convert_segment_image_to_mask(self.segmentation_cam)) + states = np.array( + self.simplifiedperception.resize_and_trimming_right_line_mask(mask) + ) + states_size = states.shape + + # print(f"{states =} and {states_size =}") + return states, states_size + + ############################################################################# + # Reset Simplified perceptcion + ############################################################################# + + def reset_sp(self): + """ + simplified perception + + """ + + if self.sensor_camera_rgb is not None: + self.destroy_all_actors_apply_batch() + + self.col_sensor = None + self.sensor_camera_rgb = None + self.sensor_camera_red_mask = None + self.front_camera_bev = None + self.front_camera_bev_mask = None + self.sensor_camera_segmentation = None + self.sensor_camera_lanedetector = None + + self.lane_sensor = None + self.obstacle_sensor = None + self.collision_hist = [] + self.lane_changing_hist = [] + self.obstacle_hist = [] + self.actor_list = [] + + ## --- Car + waypoints_town = self.world.get_map().generate_waypoints(5.0) + init_waypoint = waypoints_town[self.waypoints_init + 1] + filtered_waypoints = self.draw_waypoints( + waypoints_town, + self.waypoints_init, + self.waypoints_target, + self.waypoints_lane_id, + 2000, + ) + self.target_waypoint = filtered_waypoints[-1] + + ## ---- random init position in the whole Town: actually for test functioning porposes + if self.random_pose: + self.setup_car_random_pose() + ## -- Always same init position in a circuit predefined + elif self.alternate_pose is False: + self.setup_car_pose(self.start_alternate_pose, init=self.init_pose_number) + + ## -- Same circuit, but random init positions + else: + # self.setup_car_alternate_pose(self.start_alternate_pose) + self.setup_car_pose(self.start_alternate_pose) + + ## --- Cameras + + ## ---- RGB + # self.setup_rgb_camera() + self.setup_rgb_camera_weakref() + # self.setup_semantic_camera() + while self.sensor_camera_rgb is None: + time.sleep(0.01) + + ## ---- Red Mask + self.setup_red_mask_camera_weakref() + while self.sensor_camera_red_mask is None: + time.sleep(0.01) + + ## --- SEgmentation camera + self.setup_segmentation_camera_weakref() + # self.setup_segmentation_camera() + while self.sensor_camera_segmentation is None: + time.sleep(0.01) + + # ## ---- LAneDetector + # self.setup_lane_detector_camera_weakref() + # while self.sensor_camera_lane_detector is None: + # time.sleep(0.01) + + ## --- Detectors Sensors + self.setup_lane_invasion_sensor_weakref() + + time.sleep(1) + + ## Autopilot + # self.car.set_autopilot(True) + self.car.apply_control(carla.VehicleControl(throttle=0.0, brake=0.0)) + # self.setup_spectator() + + ########### --- calculating STATES + mask = self.simplifiedperception.preprocess_image(self.front_red_mask_camera) + right_line_in_pixels = self.simplifiedperception.calculate_right_line( + mask, self.x_row + ) + + pixels_in_state = mask.shape[1] / self.num_regions + states = [ + int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + ] + + states_size = len(states) + + # print(f"{states =} and {states_size =}") + return states, states_size + + ######################################################## + # waypoints, car pose + # + ######################################################## + + def draw_waypoints(self, spawn_points, init, target, lane_id, life_time): + """ + WArning: target always has to be the last waypoint, since it is the FINISH + """ + filtered_waypoints = [] + i = init + for waypoint in spawn_points[init + 1 : target + 2]: + filtered_waypoints.append(waypoint) + string = f"[{waypoint.road_id},{waypoint.lane_id},{i}]" + if waypoint.lane_id == lane_id: + if i != target: + self.world.debug.draw_string( + waypoint.transform.location, + f"X - {string}", + draw_shadow=False, + color=carla.Color(r=0, g=255, b=0), + life_time=life_time, + persistent_lines=True, + ) + else: + self.world.debug.draw_string( + waypoint.transform.location, + f"X - {string}", + draw_shadow=False, + color=carla.Color(r=255, g=0, b=0), + life_time=life_time, + persistent_lines=True, + ) + i += 1 + + return filtered_waypoints + + # @profile + def setup_car_pose(self, init_positions, init=None): + if init is None: + pose_init = np.random.randint(0, high=len(init_positions)) + else: + pose_init = init + + # print(f"{pose_init =}") + # print(f"{random_init = }") + # print(f"{self.start_alternate_pose = }") + # print(f"{self.start_alternate_pose[random_init][0] = }") + location = carla.Transform( + carla.Location( + x=self.start_alternate_pose[pose_init][0], + y=self.start_alternate_pose[pose_init][1], + z=self.start_alternate_pose[pose_init][2], + ), + carla.Rotation( + pitch=self.start_alternate_pose[pose_init][3], + yaw=self.start_alternate_pose[pose_init][4], + roll=self.start_alternate_pose[pose_init][5], + ), + ) + + """ these is for apply_batch_sync""" + # self.car = carla.command.SpawnActor(self.vehicle, location) + # while self.car is None: + # self.car = carla.command.SpawnActor(self.vehicle, location) + + # self.batch.append(self.car) + + """ original """ + self.car = self.world.spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(self.vehicle, location) + + # self.batch.append(self.car) + + self.actor_list.append(self.car) + + time.sleep(1) + + def setup_car_alternate_pose(self, init_positions): + random_init = np.random.randint(0, high=len(init_positions)) + # print(f"{random_init = }") + # print(f"{self.start_alternate_pose = }") + # print(f"{self.start_alternate_pose[random_init][0] = }") + location = carla.Transform( + carla.Location( + x=self.start_alternate_pose[random_init][0], + y=self.start_alternate_pose[random_init][1], + z=self.start_alternate_pose[random_init][2], + ), + carla.Rotation( + pitch=self.start_alternate_pose[random_init][3], + yaw=self.start_alternate_pose[random_init][4], + roll=self.start_alternate_pose[random_init][5], + ), + ) + + self.car = self.world.spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(self.vehicle, location) + + self.actor_list.append(self.car) + time.sleep(1) + + def setup_car_fix_pose(self, init): + """ + Town07, road 20, -1, init curves + (73.7, -10, 0.3, 0.0, -62.5, 0.0) + """ + + """ + # car_bp = self.world.get_blueprint_library().filter("vehicle.*")[0] + location = carla.Transform( + carla.Location( + x=init.transform.location.x, + y=init.transform.location.y, + z=init.transform.location.z, + ), + carla.Rotation( + pitch=init.transform.rotation.pitch, + yaw=init.transform.rotation.yaw, + roll=init.transform.rotation.roll, + ), + ) + + print(f"{init.transform.location.x = }") + print(f"{init.transform.location.y = }") + print(f"{init.lane_id = }") + print(f"{init.road_id = }") + print(f"{init.s = }") + print(f"{init.id = }") + print(f"{init.lane_width = }") + print(f"{init.lane_change = }") + print(f"{init.lane_type = }") + print(f"{init.right_lane_marking = }") + """ + + location = carla.Transform( + carla.Location(x=73.7, y=-10, z=0.300000), + carla.Rotation(pitch=0.000000, yaw=-62.5, roll=0.000000), + ) + + self.car = self.world.spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(self.vehicle, location) + + self.actor_list.append(self.car) + time.sleep(1) + + def setup_car_random_pose(self): + # car_bp = self.world.get_blueprint_library().filter("vehicle.*")[0] + location = random.choice(self.world.get_map().get_spawn_points()) + self.car = self.world.try_spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.try_spawn_actor(self.vehicle, location) + + # self.batch.append(self.car) + self.actor_list.append(self.car) + time.sleep(1) + + ################################################################## + # + # Sensors + ################################################################## + + ####################################################### + # + # Detectors + ####################################################### + + # @profile + def setup_col_sensor_weakref(self): + """weakref""" + # colsensor = self.world.get_blueprint_library().find("sensor.other.collision") + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.col_sensor = self.world.spawn_actor( + self.colsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.col_sensor) + weak_self = weakref.ref(self) + self.col_sensor.listen( + lambda event: FollowLaneDQNStaticWeatherNoTraffic._collision_data( + weak_self, event + ) + ) + # self.col_sensor.listen(self.collision_data) + + @staticmethod + def _collision_data(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + + self.collision_hist.append(event) + actor_we_collide_against = event.other_actor + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + print( + f"you have crashed with {actor_we_collide_against.type_id} with impulse {impulse} and intensity {intensity}" + ) + + def setup_col_sensor(self): + # colsensor = self.world.get_blueprint_library().find("sensor.other.collision") + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.col_sensor = self.world.spawn_actor( + self.colsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.col_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + self.col_sensor.listen(self.collision_data) + + def collision_data(self, event): + self.collision_hist.append(event) + actor_we_collide_against = event.other_actor + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + print( + f"you have crashed with {actor_we_collide_against.type_id} with impulse {impulse} and intensity {intensity}" + ) + # self.actor_list.append(actor_we_collide_against) + + def setup_lane_invasion_sensor_weakref(self): + """weakref""" + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.lane_sensor = self.world.spawn_actor( + self.laneinvsensor, transform, attach_to=self.car + ) + + # self.lane_sensor = carla.command.SpawnActor( + # self.laneinvsensor, transform, parent=self.car + # ) + + # self.batch.append(self.lane_sensor) + self.actor_list.append(self.lane_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + weak_self = weakref.ref(self) + # self.lane_sensor.listen(self.lane_changing_data) + self.lane_sensor.listen( + lambda event: FollowLaneDQNStaticWeatherNoTraffic._lane_changing( + weak_self, event + ) + ) + + @staticmethod + def _lane_changing(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + self.lane_changing_hist.append(event) + # print(f"you have changed the lane") + + def setup_lane_invasion_sensor(self): + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.lane_sensor = self.world.spawn_actor( + self.laneinvsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.lane_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + self.lane_sensor.listen(self.lane_changing_data) + + def lane_changing_data(self, event): + self.lane_changing_hist.append(event) + print(f"you have changed the lane") + + def setup_obstacle_sensor_weakref(self): + """weakref""" + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.obstacle_sensor = self.world.spawn_actor( + self.obstsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.obstacle_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + weak_self = weakref.ref(self) + # self.obstacle_sensor.listen(self.obstacle_data) + self.obstacle_sensor.listen( + lambda event: FollowLaneDQNStaticWeatherNoTraffic._obstacle_sensor( + weak_self, event + ) + ) + + @staticmethod + def _obstacle_sensor(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + self.obstacle_hist.append(event) + print(f"you have found an obstacle") + + def setup_obstacle_sensor(self): + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.obstacle_sensor = self.world.spawn_actor( + self.obstsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.obstacle_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + self.obstacle_sensor.listen(self.obstacle_data) + + def obstacle_data(self, event): + self.obstacle_hist.append(event) + print(f"you have found an obstacle") + + ####################################### + # + # spectator + ####################################### + def setup_spectator(self): + # self.spectator = self.world.get_spectator() + car_transfor = self.car.get_transform() + # world_snapshot = self.world.wait_for_tick() + # actor_snapshot = world_snapshot.find(self.car.id) + # Set spectator at given transform (vehicle transform) + # self.spectator.set_transform(actor_snapshot.get_transform()) + self.spectator.set_transform( + carla.Transform( + car_transfor.location + carla.Location(z=60), + carla.Rotation(pitch=-90, roll=-90), + ) + ) + # self.actor_list.append(self.spectator) + + ###################################################### + # + # Cameras + ###################################################### + + ######################################## + # + # RGB + ######################################## + + # @profile + def setup_rgb_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_rgb = self.world.spawn_actor( + self.rgb_cam, transform, attach_to=self.car + ) + + # self.sensor_camera_rgb = carla.command.SpawnActor( + # self.rgb_cam, transform, parent=self.car + # ) + + # self.batch.append(self.sensor_camera_rgb) + + self.actor_list.append(self.sensor_camera_rgb) + # print(f"{len(self.actor_list) = }") + weak_self = weakref.ref(self) + # self.sensor_camera_rgb.listen(self.save_rgb_image) + self.sensor_camera_rgb.listen( + lambda event: FollowLaneDQNStaticWeatherNoTraffic._rgb_image( + weak_self, event + ) + ) + + @staticmethod + def _rgb_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + + image = np.array(image.raw_data) + image = image.reshape((480, 640, 4)) + # image = image.reshape((512, 1024, 4)) + image = image[:, :, :3] + # self._data_dict["image"] = image3 + self.front_rgb_camera = image + + def setup_rgb_camera(self): + # print("enter setup_rg_camera") + # rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # rgb_cam.set_attribute("image_size_x", f"{self.width}") + # rgb_cam.set_attribute("image_size_y", f"{self.height}") + # rgb_cam.set_attribute("fov", f"110") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_rgb = self.world.spawn_actor( + self.rgb_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_rgb) + # print(f"{len(self.actor_list) = }") + self.sensor_camera_rgb.listen(self.save_rgb_image) + + def save_rgb_image(self, data: carla.Image): + if not isinstance(data, carla.Image): + raise ValueError("Argument must be a carla.Image") + image = np.array(data.raw_data) + image = image.reshape((480, 640, 4)) + image = image[:, :, :3] + # self._data_dict["image"] = image3 + self.front_rgb_camera = image + + def _save_rgb_image(self, image): + # t_start = self.timer.time() + + image.convert(carla.ColorConverter.Raw) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + # if self.display_man.render_enabled(): + # self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + # cv2.imshow("", array) + # cv2.waitKey(1) + self.front_rgb_camera = array + + ################## + # + # Red Mask Camera + ################## + def setup_red_mask_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_red_mask = self.world.spawn_actor( + self.red_mask_cam, transform, attach_to=self.car + ) + + # self.sensor_camera_red_mask = carla.command.SpawnActor( + # self.red_mask_cam, transform, parent=self.car + # ) + + # self.batch.append(self.sensor_camera_red_mask) + self.actor_list.append(self.sensor_camera_red_mask) + weak_self = weakref.ref(self) + + # start_camera_red_mask = time.time() + self.sensor_camera_red_mask.listen( + lambda event: FollowLaneDQNStaticWeatherNoTraffic._red_mask_semantic_image( + weak_self, event + ) + ) + # end_camera_red_mask = time.time() + # print( + # f"\n====> sensor_camera_red_mask time processing: {end_camera_red_mask - start_camera_red_mask = }" + # ) + + # self.sensor_camera_red_mask.listen(self.save_red_mask_semantic_image) + + @staticmethod + def _red_mask_semantic_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + hsv_nemo = cv2.cvtColor(array, cv2.COLOR_RGB2HSV) + + if ( + self.world.get_map().name == "Carla/Maps/Town07" + or self.world.get_map().name == "Carla/Maps/Town04" + or self.world.get_map().name == "Carla/Maps/Town07_Opt" + or self.world.get_map().name == "Carla/Maps/Town04_Opt" + ): + light_sidewalk = (42, 200, 233) + dark_sidewalk = (44, 202, 235) + else: + light_sidewalk = (151, 217, 243) + dark_sidewalk = (153, 219, 245) + + light_pavement = (149, 127, 127) + dark_pavement = (151, 129, 129) + + mask_sidewalk = cv2.inRange(hsv_nemo, light_sidewalk, dark_sidewalk) + # result_sidewalk = cv2.bitwise_and(array, array, mask=mask_sidewalk) + + mask_pavement = cv2.inRange(hsv_nemo, light_pavement, dark_pavement) + # result_pavement = cv2.bitwise_and(array, array, mask=mask_pavement) + + # Adjust according to your adjacency requirement. + kernel = np.ones((3, 3), dtype=np.uint8) + + # Dilating masks to expand boundary. + color1_mask = cv2.dilate(mask_sidewalk, kernel, iterations=1) + color2_mask = cv2.dilate(mask_pavement, kernel, iterations=1) + + # Required points now will have both color's mask val as 255. + common = cv2.bitwise_and(color1_mask, color2_mask) + SOME_THRESHOLD = 0 + + # Common is binary np.uint8 image, min = 0, max = 255. + # SOME_THRESHOLD can be anything within the above range. (not needed though) + # Extract/Use it in whatever way you want it. + intersection_points = np.where(common > SOME_THRESHOLD) + + # Say you want these points in a list form, then you can do this. + pts_list = [[r, c] for r, c in zip(*intersection_points)] + # print(pts_list) + + # for x, y in pts_list: + # image_2[x][y] = (255, 0, 0) + + # red_line_mask = np.zeros((400, 500, 3), dtype=np.uint8) + red_line_mask = np.zeros((image.height, image.width, 3), dtype=np.uint8) + + for x, y in pts_list: + red_line_mask[x][y] = (255, 0, 0) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + red_line_mask = cv2.cvtColor(red_line_mask, cv2.COLOR_BGR2RGB) + self.front_red_mask_camera = red_line_mask + + def setup_red_mask_camera(self): + # self.red_mask_cam = self.world.get_blueprint_library().find( + # "sensor.camera.semantic_segmentation" + # ) + # self.red_mask_cam.set_attribute("image_size_x", f"{self.width}") + # self.red_mask_cam.set_attribute("image_size_y", f"{self.height}") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_red_mask = self.world.spawn_actor( + self.red_mask_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_red_mask) + self.sensor_camera_red_mask.listen(self.save_red_mask_semantic_image) + + def save_red_mask_semantic_image(self, image): + # t_start = self.timer.time() + # print(f"en red_mask calbback") + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + hsv_nemo = cv2.cvtColor(array, cv2.COLOR_RGB2HSV) + + if ( + self.world.get_map().name == "Carla/Maps/Town07" + or self.world.get_map().name == "Carla/Maps/Town04" + ): + light_sidewalk = (42, 200, 233) + dark_sidewalk = (44, 202, 235) + else: + light_sidewalk = (151, 217, 243) + dark_sidewalk = (153, 219, 245) + + light_pavement = (149, 127, 127) + dark_pavement = (151, 129, 129) + + mask_sidewalk = cv2.inRange(hsv_nemo, light_sidewalk, dark_sidewalk) + # result_sidewalk = cv2.bitwise_and(array, array, mask=mask_sidewalk) + + mask_pavement = cv2.inRange(hsv_nemo, light_pavement, dark_pavement) + # result_pavement = cv2.bitwise_and(array, array, mask=mask_pavement) + + # Adjust according to your adjacency requirement. + kernel = np.ones((3, 3), dtype=np.uint8) + + # Dilating masks to expand boundary. + color1_mask = cv2.dilate(mask_sidewalk, kernel, iterations=1) + color2_mask = cv2.dilate(mask_pavement, kernel, iterations=1) + + # Required points now will have both color's mask val as 255. + common = cv2.bitwise_and(color1_mask, color2_mask) + SOME_THRESHOLD = 0 + + # Common is binary np.uint8 image, min = 0, max = 255. + # SOME_THRESHOLD can be anything within the above range. (not needed though) + # Extract/Use it in whatever way you want it. + intersection_points = np.where(common > SOME_THRESHOLD) + + # Say you want these points in a list form, then you can do this. + pts_list = [[r, c] for r, c in zip(*intersection_points)] + # print(pts_list) + + # for x, y in pts_list: + # image_2[x][y] = (255, 0, 0) + + # red_line_mask = np.zeros((400, 500, 3), dtype=np.uint8) + red_line_mask = np.zeros((image.height, image.width, 3), dtype=np.uint8) + + for x, y in pts_list: + red_line_mask[x][y] = (255, 0, 0) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + red_line_mask = cv2.cvtColor(red_line_mask, cv2.COLOR_BGR2RGB) + self.front_red_mask_camera = red_line_mask + + ################## + # + # BEV + ################## + def setup_bev_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_bev_camera = self.world.spawn_actor( + self.bev_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_bev_camera) + # print(f"{len(self.actor_list) = }") + weak_self = weakref.ref(self) + # self.sensor_bev_camera.listen(self.save_bev_image) + self.sensor_bev_camera.listen( + lambda event: FollowLaneDQNStaticWeatherNoTraffic._bev_image( + weak_self, event + ) + ) + + @staticmethod + def _bev_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + + car_bp = self.world.get_actors().filter("vehicle.*")[0] + # car_bp = self.vehicle + birdview = self.birdview_producer.produce( + agent_vehicle=car_bp # carla.Actor (spawned vehicle) + ) + image = BirdViewProducer.as_rgb(birdview) + hsv_nemo = cv2.cvtColor(image, cv2.COLOR_RGB2HSV) + + # Extract central line + light_center_line = (17, 134, 232) + dark_center_line = (19, 136, 234) + mask_center_line = cv2.inRange(hsv_nemo, light_center_line, dark_center_line) + result = cv2.bitwise_and(hsv_nemo, hsv_nemo, mask=mask_center_line) + + # image = np.rot90(image) + image = np.array(image) + result = np.array(result) + if image.shape[0] != image.shape[1]: + if image.shape[0] > image.shape[1]: + difference = image.shape[0] - image.shape[1] + extra_left, extra_right = int(difference / 2), int(difference / 2) + extra_top, extra_bottom = 0, 0 + else: + difference = image.shape[1] - image.shape[0] + extra_left, extra_right = 0, 0 + extra_top, extra_bottom = int(difference / 2), int(difference / 2) + image = np.pad( + image, + ((extra_top, extra_bottom), (extra_left, extra_right), (0, 0)), + mode="constant", + constant_values=0, + ) + image = np.pad( + image, + ((100, 100), (50, 50), (0, 0)), + mode="constant", + constant_values=0, + ) + + self.front_camera_bev = image + self.front_camera_bev_mask = result + + def setup_bev_camera(self): + # print("enter setup_rg_camera") + # rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # rgb_cam.set_attribute("image_size_x", f"{self.width}") + # rgb_cam.set_attribute("image_size_y", f"{self.height}") + # rgb_cam.set_attribute("fov", f"110") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_bev_camera = self.world.spawn_actor( + self.bev_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_bev_camera) + # print(f"{len(self.actor_list) = }") + self.sensor_bev_camera.listen(self.save_bev_image) + + def save_bev_image(self, image): + car_bp = self.world.get_actors().filter("vehicle.*")[0] + # car_bp = self.vehicle + birdview = self.birdview_producer.produce( + agent_vehicle=car_bp # carla.Actor (spawned vehicle) + ) + image = BirdViewProducer.as_rgb(birdview) + hsv_nemo = cv2.cvtColor(image, cv2.COLOR_RGB2HSV) + + # Extract central line + light_center_line = (17, 134, 232) + dark_center_line = (19, 136, 234) + mask_center_line = cv2.inRange(hsv_nemo, light_center_line, dark_center_line) + result = cv2.bitwise_and(hsv_nemo, hsv_nemo, mask=mask_center_line) + + # image = np.rot90(image) + image = np.array(image) + result = np.array(result) + if image.shape[0] != image.shape[1]: + if image.shape[0] > image.shape[1]: + difference = image.shape[0] - image.shape[1] + extra_left, extra_right = int(difference / 2), int(difference / 2) + extra_top, extra_bottom = 0, 0 + else: + difference = image.shape[1] - image.shape[0] + extra_left, extra_right = 0, 0 + extra_top, extra_bottom = int(difference / 2), int(difference / 2) + image = np.pad( + image, + ((extra_top, extra_bottom), (extra_left, extra_right), (0, 0)), + mode="constant", + constant_values=0, + ) + image = np.pad( + image, + ((100, 100), (50, 50), (0, 0)), + mode="constant", + constant_values=0, + ) + + self.front_camera_bev = image + self.front_camera_bev_mask = result + + ################## + # + # Segmentation Camera + ################## + + def setup_segmentation_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_segmentation = self.world.spawn_actor( + self.segm_cam, transform, attach_to=self.car + ) + + # self.sensor_camera_segmentation = carla.command.SpawnActor( + # self.segm_cam, transform, parent=self.car + # ) + + # self.batch.append(self.sensor_camera_segmentation) + self.actor_list.append(self.sensor_camera_segmentation) + # print(f"{len(self.actor_list) = }") + weak_self = weakref.ref(self) + # self.sensor_camera_segmentation.listen(self.save_segmentation_image) + self.sensor_camera_segmentation.listen( + lambda event: FollowLaneDQNStaticWeatherNoTraffic._segmentation_image( + weak_self, event + ) + ) + + @staticmethod + def _segmentation_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + self.segmentation_cam = array + + def setup_segmentation_camera(self): + # print("enter setup_rg_camera") + # rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # rgb_cam.set_attribute("image_size_x", f"{self.width}") + # rgb_cam.set_attribute("image_size_y", f"{self.height}") + # rgb_cam.set_attribute("fov", f"110") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_segmentation = self.world.spawn_actor( + self.segm_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_segmentation) + # print(f"{len(self.actor_list) = }") + self.sensor_camera_segmentation.listen(self.save_segmentation_image) + + def save_segmentation_image(self, image: carla.Image): + # t_start = self.timer.time() + + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + self.segmentation_cam = array + # if self.display_man.render_enabled(): + # self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + ################## + # + # Destroyers + ################## + + def destroy_all_actors_apply_batch(self): + print("\ndestroying %d actors with apply_batch method" % len(self.actor_list)) + self.client.apply_batch( + [carla.command.DestroyActor(x) for x in self.actor_list[::-1]] + ) + + def destroy_all_actors(self): + print("\ndestroying %d actors with destroy() method" % len(self.actor_list)) + for actor in self.actor_list[::-1]: + actor.destroy() + + #################################################################### + # + # Step + # + #################################################################### + + def step(self, action): + # from rl_studio.envs.gazebo.f1.models.step import ( + # StepFollowLane, + # ) + + if ( + self.state_space == "image" + and self.action_space != "continuous" + and self.states_entry == "only_right_line" + ): + return self.step_followlane_state_image_actions_discretes_only_right_line( + action + ) + # Faltan 2 mas por crear + + elif ( + self.state_space != "image" + and self.action_space != "continuous" + and self.states_entry == "only_right_line" + ): + self.step_followlane_state_sp_actions_discretes(action) + + else: # aqui faltaria el state_sp_actions_discretes_lane_detector, pero antes hay qye crear los otros 2 + pass + # self.step_followlane_state_sp_actions_discretes(action) + + ######################################################## + # step + # state: image + # actions: continuous + # only right line + ####################################################### + def step_followlane_state_image_actions_discretes_only_right_line(self, action): + """ + state: Image + actions: continuous + only right line + """ + + self.control_discrete_actions_only_right(action) + + ########### --- calculating STATES + # start_camera_red_mask = time.time() + mask = self.simplifiedperception.preprocess_image(self.front_red_mask_camera) + + ########### --- Calculating center ONLY with right line + right_line_in_pixels = self.simplifiedperception.calculate_right_line( + mask, self.x_row + ) + + image_center = mask.shape[1] // 2 + dist = [ + image_center - right_line_in_pixels[i] + for i, _ in enumerate(right_line_in_pixels) + ] + dist_normalized = [ + float((image_center - right_line_in_pixels[i]) / image_center) + for i, _ in enumerate(right_line_in_pixels) + ] + + pixels_in_state = mask.shape[1] // self.num_regions + state_right_lines = [ + i for i in range(1, mask.shape[1]) if i % pixels_in_state == 0 + ] + + ## STATES + + # states = [ + # int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + # ] + # states = np.array(self.convert_segment_image_to_mask(self.segmentation_cam)) + states = np.array( + self.simplifiedperception.resize_and_trimming_right_line_mask(mask) + ) + + # print(f"{lane_centers_in_pixels = }") + + ## -------- Ending Step()... + done = False + ground_truth_normal_values = [ + correct_normalized_distance[value] for i, value in enumerate(self.x_row) + ] + # reward, done = self.autocarlarewards.rewards_right_line( + # dist_normalized, ground_truth_normal_values, self.params + # ) + reward, done = self.autocarlarewards.rewards_sigmoid_only_right_line( + dist_normalized, ground_truth_normal_values + ) + + ## -------- ... or Finish by... + if len(self.collision_hist) > 0: # crashed you, baby + done = True + # reward = -100 + print(f"crash") + + self.is_finish, self.dist_to_finish = AutoCarlaUtils.finish_fix_number_target( + self.params["location"], + self.finish_alternate_pose, + self.finish_pose_number, + self.max_target_waypoint_distance, + ) + if self.is_finish: + print(f"Finish!!!!") + done = True + # reward = 100 + + # if len(self.lane_changing_hist) > 1: # you leave the lane + # done = True + # reward = -100 + # print(f"out of lane") + + ground_truth_pixel_values = [ + correct_pixel_distance[value] for i, value in enumerate(self.x_row) + ] + + AutoCarlaUtils.show_image( + "states", + states, + 600, + 400, + ) + + # AutoCarlaUtils.show_image( + # "segmentation_cam", + # self.segmentation_cam, + # 1200, + # 400, + # ) + + # AutoCarlaUtils.show_image( + # "self.sensor_camera_segmentation", + # self.sensor_camera_segmentation, + # 1200, + # 1000, + # ) + + AutoCarlaUtils.show_image_only_right_line( + "mask", + mask, + 1, + right_line_in_pixels, + ground_truth_pixel_values, + dist_normalized, + [4, 4, 4, 4], + self.x_row, + 600, + 10, + state_right_lines, + ) + + AutoCarlaUtils.show_image_only_right_line( + "front RGB", + self.front_rgb_camera[(self.front_rgb_camera.shape[0] // 2) :], + 1, + right_line_in_pixels, + ground_truth_pixel_values, + dist_normalized, + [4, 4, 4, 4], + self.x_row, + 1250, + 10, + state_right_lines, + ) + + render_params( + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + steering_angle=self.params["steering_angle"], + # Steering_angle=self.params["Steering_angle"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + height=self.params["height"], + action=action, + speed_kmh=self.params["speed"], + acceleration_ms2=self.params["Acceleration"], + _="------------------------", + # states=states, + # lane_centers_in_pixels=lane_centers_in_pixels, + # image_center=image_center, + right_line_in_pixels=right_line_in_pixels, + dist=dist, + dist_normalized=dist_normalized, + ground_truth_normal_values=ground_truth_normal_values, + reward=reward, + done=done, + distance_to_finish=self.dist_to_finish, + num_self_lane_change_hist=len(self.lane_changing_hist), + num_self_collision_hist=len(self.collision_hist), + num_self_obstacle_hist=len(self.obstacle_hist), + ) + """ + print_messages( + "in step()", + height=mask.shape[0], + width=mask.shape[1], + action=action, + velocity=self.params["speed"], + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + _="------------------------", + states=states, + right_line_in_pixels=right_line_in_pixels, + dist=dist, + dist_normalized=dist_normalized, + # self_perfect_distance_pixels=self.perfect_distance_pixels, + # self_perfect_distance_normalized=self.perfect_distance_normalized, + # error=error, + done=done, + reward=reward, + distance_to_finish=self.dist_to_finish, + self_collision_hist=self.collision_hist, + num_self_obstacle_hist=len(self.obstacle_hist), + num_self_lane_change_hist=len(self.lane_changing_hist), + ) + """ + return states, reward, done, {} + + ######################################################## + # step + # state: image + # actions: continuous + # only right line + ####################################################### + def step_followlane_state_sp_actions_discretes(self, action): + """ + state: sp + actions: continuous + only right line + """ + + self.control_discrete_actions_only_right(action) + + ########### --- calculating STATES + # start_camera_red_mask = time.time() + mask = self.simplifiedperception.preprocess_image(self.front_red_mask_camera) + + ########### --- Calculating center ONLY with right line + right_line_in_pixels = self.simplifiedperception.calculate_right_line( + mask, self.x_row + ) + + image_center = mask.shape[1] // 2 + dist = [ + image_center - right_line_in_pixels[i] + for i, _ in enumerate(right_line_in_pixels) + ] + dist_normalized = [ + float((image_center - right_line_in_pixels[i]) / image_center) + for i, _ in enumerate(right_line_in_pixels) + ] + + pixels_in_state = mask.shape[1] // self.num_regions + state_right_lines = [ + i for i in range(1, mask.shape[1]) if i % pixels_in_state == 0 + ] + + ## STATES + + states = [ + int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + ] + + ## -------- Ending Step()... + done = False + ground_truth_normal_values = [ + correct_normalized_distance[value] for i, value in enumerate(self.x_row) + ] + # reward, done = self.autocarlarewards.rewards_right_line( + # dist_normalized, ground_truth_normal_values, self.params + # ) + reward, done = self.autocarlarewards.rewards_sigmoid_only_right_line( + dist_normalized, ground_truth_normal_values + ) + + ## -------- ... or Finish by... + if len(self.collision_hist) > 0: # crashed you, baby + done = True + # reward = -100 + print(f"crash") + + self.is_finish, self.dist_to_finish = AutoCarlaUtils.finish_fix_number_target( + self.params["location"], + self.finish_alternate_pose, + self.finish_pose_number, + self.max_target_waypoint_distance, + ) + if self.is_finish: + print(f"Finish!!!!") + done = True + # reward = 100 + + # if len(self.lane_changing_hist) > 1: # you leave the lane + # done = True + # reward = -100 + # print(f"out of lane") + + ground_truth_pixel_values = [ + correct_pixel_distance[value] for i, value in enumerate(self.x_row) + ] + + AutoCarlaUtils.show_image( + "states", + states, + 600, + 400, + ) + + # AutoCarlaUtils.show_image( + # "segmentation_cam", + # self.segmentation_cam, + # 1200, + # 400, + # ) + + # AutoCarlaUtils.show_image( + # "self.sensor_camera_segmentation", + # self.sensor_camera_segmentation, + # 1200, + # 1000, + # ) + + AutoCarlaUtils.show_image_only_right_line( + "mask", + mask, + 1, + right_line_in_pixels, + ground_truth_pixel_values, + dist_normalized, + [4, 4, 4, 4], + self.x_row, + 600, + 10, + state_right_lines, + ) + + AutoCarlaUtils.show_image_only_right_line( + "front RGB", + self.front_rgb_camera[(self.front_rgb_camera.shape[0] // 2) :], + 1, + right_line_in_pixels, + ground_truth_pixel_values, + dist_normalized, + [4, 4, 4, 4], + self.x_row, + 1250, + 10, + state_right_lines, + ) + + render_params( + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + steering_angle=self.params["steering_angle"], + # Steering_angle=self.params["Steering_angle"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + height=self.params["height"], + action=action, + speed_kmh=self.params["speed"], + acceleration_ms2=self.params["Acceleration"], + _="------------------------", + # states=states, + # lane_centers_in_pixels=lane_centers_in_pixels, + # image_center=image_center, + right_line_in_pixels=right_line_in_pixels, + dist=dist, + dist_normalized=dist_normalized, + ground_truth_normal_values=ground_truth_normal_values, + reward=reward, + done=done, + distance_to_finish=self.dist_to_finish, + num_self_lane_change_hist=len(self.lane_changing_hist), + num_self_collision_hist=len(self.collision_hist), + num_self_obstacle_hist=len(self.obstacle_hist), + ) + """ + print_messages( + "in step()", + height=mask.shape[0], + width=mask.shape[1], + action=action, + velocity=self.params["speed"], + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + _="------------------------", + states=states, + right_line_in_pixels=right_line_in_pixels, + dist=dist, + dist_normalized=dist_normalized, + # self_perfect_distance_pixels=self.perfect_distance_pixels, + # self_perfect_distance_normalized=self.perfect_distance_normalized, + # error=error, + done=done, + reward=reward, + distance_to_finish=self.dist_to_finish, + self_collision_hist=self.collision_hist, + num_self_obstacle_hist=len(self.obstacle_hist), + num_self_lane_change_hist=len(self.lane_changing_hist), + ) + """ + return states, reward, done, {} + + ################################################## + # + # Control + ################################################### + + def control_discrete_actions_only_right(self, action): + t = self.car.get_transform() + v = self.car.get_velocity() + c = self.car.get_control() + w = self.car.get_angular_velocity() + a = self.car.get_acceleration() + self.params["speed"] = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + self.params["steering_angle"] = w + # self.params["Steering_angle"] = steering_angle + self.params["Steer"] = c.steer + self.params["location"] = (t.location.x, t.location.y) + self.params["Throttle"] = c.throttle + self.params["Brake"] = c.brake + self.params["height"] = t.location.z + self.params["Acceleration"] = math.sqrt(a.x**2 + a.y**2 + a.z**2) + + ## Applied throttle, brake and steer + curr_speed = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + target_speed = 30 + # throttle_low_limit = 0.0 #0.1 for low speeds + brake = 0 + throttle = self.actions[action][0] + steer = self.actions[action][1] + + if curr_speed > target_speed: + throttle = 0.45 + + self.car.apply_control( + carla.VehicleControl(throttle=throttle, steer=steer, brake=brake) + ) + + def control_only_right(self, action): + """ + DDPG with continuos actions + """ + + t = self.car.get_transform() + v = self.car.get_velocity() + c = self.car.get_control() + w = self.car.get_angular_velocity() + a = self.car.get_acceleration() + self.params["speed"] = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + self.params["steering_angle"] = w + # self.params["Steering_angle"] = steering_angle + self.params["Steer"] = c.steer + self.params["location"] = (t.location.x, t.location.y) + self.params["Throttle"] = c.throttle + self.params["Brake"] = c.brake + self.params["height"] = t.location.z + self.params["Acceleration"] = math.sqrt(a.x**2 + a.y**2 + a.z**2) + + ## Applied throttle, brake and steer + curr_speed = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + target_speed = 30 + # throttle_low_limit = 0.0 #0.1 for low speeds + # vel_error = curr_speed / target_speed - 1 + # limit = 0.0 + # if vel_error > limit: + # brake = 1 + # throttle = 0.45 + # else: + # throttle = 0.7 + + brake = 0 + throttle = action[0][0] + steer = action[0][1] + + if curr_speed > target_speed: + throttle = 0.45 + # else: + # throttle = 0.8 + + # clip_throttle = self.clip_throttle( + # throttle_upper_limit, curr_speed, target_speed, throttle_low_limit + # ) + + # self.car.apply_control( + # carla.VehicleControl(throttle=clip_throttle, steer=steer) + # ) + + self.car.apply_control( + carla.VehicleControl(throttle=throttle, steer=steer, brake=brake) + ) + + ######################################################## + # utils + # + ######################################################## + + def __del__(self): + print("__del__ called") + + def checking_carla_server(self, town): + # print(f"checking Carla Server...") + + try: + ps_output = ( + subprocess.check_output(["ps", "-Af"]).decode("utf-8").strip("\n") + ) + except subprocess.CalledProcessError as ce: + print("SimulatorEnv: exception raised executing ps command {}".format(ce)) + sys.exit(-1) + + if ( + (ps_output.count("CarlaUE4.sh") == 0) + or (ps_output.count("CarlaUE4-Linux-Shipping") == 0) + or (ps_output.count("CarlaUE4-Linux-") == 0) + ): + try: + carla_root = os.environ["CARLA_ROOT"] + # print(f"{carla_root = }\n") + carla_exec = f"{carla_root}/CarlaUE4.sh" + + with open("/tmp/.carlalaunch_stdout.log", "w") as out, open( + "/tmp/.carlalaunch_stderr.log", "w" + ) as err: + print_messages( + "launching Carla Server again...", + ) + subprocess.Popen( + [carla_exec, "-prefernvidia"], stdout=out, stderr=err + ) + time.sleep(5) + self.world = self.client.load_world(town) + + except subprocess.CalledProcessError as ce: + print( + "SimulatorEnv: exception raised executing killall command for CARLA server {}".format( + ce + ) + ) diff --git a/rl_studio/envs/carla/followlane/followlane_qlearn.py b/rl_studio/envs/carla/followlane/followlane_qlearn.py index 108ab1d4c..dad4b915d 100644 --- a/rl_studio/envs/carla/followlane/followlane_qlearn.py +++ b/rl_studio/envs/carla/followlane/followlane_qlearn.py @@ -1,21 +1,34 @@ from collections import Counter +import copy +from datetime import datetime, timedelta import math +import os +import subprocess import time +import sys + import carla +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions from cv_bridge import CvBridge import cv2 from geometry_msgs.msg import Twist +import matplotlib.pyplot as plt +from memory_profiler import profile import numpy as np +import pygame import random -from datetime import datetime, timedelta import weakref import rospy from sensor_msgs.msg import Image +import torch +import torchvision from rl_studio.agents.utils import ( print_messages, + render_params, ) from rl_studio.envs.carla.followlane.followlane_env import FollowLaneEnv +from rl_studio.envs.carla.followlane.images import LaneDetector from rl_studio.envs.carla.followlane.settings import FollowLaneCarlaConfig from rl_studio.envs.carla.followlane.utils import AutoCarlaUtils @@ -25,268 +38,654 @@ SensorManager, CustomTimer, ) -import pygame + from rl_studio.envs.carla.utils.global_route_planner import ( GlobalRoutePlanner, ) +correct_normalized_distance = { + 20: -0.07, + 30: -0.1, + 35: -0.12, + 40: -0.13, + 45: -0.15, + 50: -0.17, + 60: -0.2, + 70: -0.23, + 80: -0.26, + 90: -0.3, + 100: -0.33, + 110: -0.36, + 120: -0.4, + 130: -0.42, + 140: -0.46, + 150: -0.49, + 160: -0.52, + 170: -0.56, + 180: -0.59, + 190: -0.62, + 200: -0.65, + 210: -0.69, + 220: -0.72, +} +correct_pixel_distance = { + 20: 343, + 30: 353, + 35: 358, + 40: 363, + 45: 368, + 50: 374, + 60: 384, + 70: 394, + 80: 404, + 90: 415, + 100: 425, + 110: 436, + 120: 446, + 130: 456, + 140: 467, + 150: 477, + 160: 488, + 170: 498, + 180: 508, + 190: 518, + 200: 528, + 210: 540, + 220: 550, +} + class FollowLaneQlearnStaticWeatherNoTraffic(FollowLaneEnv): def __init__(self, **config): - - ###### init F1env + ## --------------- init F1env FollowLaneEnv.__init__(self, **config) - ###### init class variables + ## --------------- init class variables FollowLaneCarlaConfig.__init__(self, **config) - # self.display_manager = None - # self.vehicle = None - # self.actor_list = [] self.timer = CustomTimer() self.client = carla.Client( config["carla_server"], config["carla_client"], ) - self.client.set_timeout(5.0) + self.client.set_timeout(3.0) print(f"\n maps in carla 0.9.13: {self.client.get_available_maps()}\n") + """ + self.world = self.client.get_world() + self.world = self.client.load_world(config["town"]) + settings = self.world.get_settings() + settings.fixed_delta_seconds = 0.05 + if config["sync"]: + settings.synchronous_mode = True + else: + settings.synchronous_mode = False + self.world.apply_settings(settings) + + """ + self.world = self.client.load_world(config["town"]) + if config["town"] == "Town07_Opt": + self.world.unload_map_layer(carla.MapLayer.Buildings) + self.world.unload_map_layer(carla.MapLayer.Decals) + self.world.unload_map_layer(carla.MapLayer.Foliage) + self.world.unload_map_layer(carla.MapLayer.Particles) + self.world.unload_map_layer(carla.MapLayer.Props) + self.original_settings = self.world.get_settings() self.traffic_manager = self.client.get_trafficmanager(8000) settings = self.world.get_settings() settings.fixed_delta_seconds = 0.05 if config["sync"]: self.traffic_manager.set_synchronous_mode(True) + # settings.synchronous_mode = True else: self.traffic_manager.set_synchronous_mode(False) + # settings.synchronous_mode = False + self.world.apply_settings(settings) - # self.camera = None - # self.vehicle = None - # self.display = None - # self.image = None + ### Weather + weather = carla.WeatherParameters( + cloudiness=70.0, precipitation=0.0, sun_altitude_angle=70.0 + ) + self.world.set_weather(weather) + + """este no funciona + self.world = self.client.load_world(config["town"]) + # self.original_settings = self.world.get_settings() + # self.traffic_manager = self.client.get_trafficmanager(8000) + settings = self.world.get_settings() + settings.fixed_delta_seconds = 0.05 + if config["sync"]: + settings.synchronous_mode = True + else: + settings.synchronous_mode = False + self.world.apply_settings(settings) + """ ## -- display manager - self.display_manager = DisplayManager( - grid_size=[2, 3], - window_size=[1500, 800], - ) + # self.display_manager = DisplayManager( + # grid_size=[2, 2], + # window_size=[900, 600], + # ) + ## --------------- Blueprint --------------- + self.blueprint_library = self.world.get_blueprint_library() + ## --------------- Car --------------- + self.vehicle = self.world.get_blueprint_library().filter("vehicle.*")[0] self.car = None - + ## --------------- collision sensor --------------- + self.colsensor = self.world.get_blueprint_library().find( + "sensor.other.collision" + ) + self.collision_hist = [] + self.col_sensor = None + ## --------------- Lane invasion sensor --------------- + self.laneinvsensor = self.world.get_blueprint_library().find( + "sensor.other.lane_invasion" + ) + self.lane_changing_hist = [] + self.lane_sensor = None + ## --------------- Obstacle sensor --------------- + self.obstsensor = self.world.get_blueprint_library().find( + "sensor.other.obstacle" + ) + self.obstacle_hist = [] + self.obstacle_sensor = None + ## --------------- RGB camera --------------- + self.rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + self.rgb_cam.set_attribute("image_size_x", f"{self.width}") + self.rgb_cam.set_attribute("image_size_y", f"{self.height}") + self.rgb_cam.set_attribute("fov", f"110") + self.sensor_camera_rgb = None + self.front_rgb_camera = None + ## --------------- RedMask Camera --------------- + self.red_mask_cam = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + self.red_mask_cam.set_attribute("image_size_x", f"{self.width}") + self.red_mask_cam.set_attribute("image_size_y", f"{self.height}") + self.red_mask_cam.set_attribute("fov", f"110") + self.sensor_camera_red_mask = None + self.front_red_mask_camera = None + ## --------------- BEV camera --------------- + """ + self.birdview_producer = BirdViewProducer( + self.client, # carla.Client + target_size=PixelDimensions(width=100, height=300), + pixels_per_meter=10, + crop_type=BirdViewCropType.FRONT_AREA_ONLY, + ) + self.bev_cam = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + self.bev_cam.set_attribute("image_size_x", f"{self.width}") + self.bev_cam.set_attribute("image_size_y", f"{self.height}") + self.front_camera_bev = None + self.front_camera_bev_mask = None + """ + ## --------------- Segmentation Camera --------------- + self.segm_cam = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + self.segm_cam.set_attribute("image_size_x", f"{self.width}") + self.segm_cam.set_attribute("image_size_y", f"{self.height}") + self.sensor_camera_segmentation = None + self.segmentation_cam = None + + ## --------------- LaneDetector Camera --------------- + # self.lanedetector_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # self.lanedetector_cam.set_attribute("image_size_x", f"{self.width}") + # self.lanedetector_cam.set_attribute("image_size_y", f"{self.height}") + # self.lanedetector_cam.set_attribute("fov", f"110") + # self.sensor_camera_lanedetector = None + # self.front_lanedetector_camera = None + self._detector = LaneDetector("models/fastai_torch_lane_detector_model.pth") + # torch.cuda.empty_cache() + # self.model = torch.load("models/fastai_torch_lane_detector_model.pth") + # self.model.eval() + + ## --------------- more --------------- self.perfect_distance_pixels = None self.perfect_distance_normalized = None + # self._control = carla.VehicleControl() + self.params = {} + self.target_waypoint = None + + self.spectator = self.world.get_spectator() + # self.spectator = None + self.actor_list = [] + self.is_finish = None + self.dist_to_finish = None + ################################################################################# + # + # Reset + ################################################################################# + # @profile + """ def reset(self): + if self.states_entry == "one_only_right_line": + # print("entro en one_point") + return self.reset_one_point_only_right_line() + """ + # def reset_one_point_only_right_line(self): + def reset(self): + if self.sensor_camera_rgb is not None: + self.destroy_all_actors_apply_batch() + + self.col_sensor = None + self.sensor_camera_rgb = None + self.sensor_camera_red_mask = None + self.front_camera_bev = None + self.front_camera_bev_mask = None + self.sensor_camera_segmentation = None + self.sensor_camera_lanedetector = None + + self.lane_sensor = None + self.obstacle_sensor = None self.collision_hist = [] + self.lane_changing_hist = [] + self.obstacle_hist = [] self.actor_list = [] - spectator = self.world.get_spectator() ## --- Car waypoints_town = self.world.get_map().generate_waypoints(5.0) init_waypoint = waypoints_town[self.waypoints_init + 1] + filtered_waypoints = self.draw_waypoints( + waypoints_town, + self.waypoints_init, + self.waypoints_target, + self.waypoints_lane_id, + 2000, + ) + self.target_waypoint = filtered_waypoints[-1] - if self.alternate_pose: + ## ---- random init position in the whole Town: actually for test functioning porposes + if self.random_pose: self.setup_car_random_pose() - elif self.waypoints_target is not None: - # waypoints = self.get_waypoints() - filtered_waypoints = self.draw_waypoints( - waypoints_town, - self.waypoints_init, - self.waypoints_target, - self.waypoints_lane_id, - 2000, - ) - self.setup_car_fix_pose(init_waypoint) - - else: # TODO: hacer en el caso que se quiera poner el target con .next() - waypoints_lane = init_waypoint.next_until_lane_end(1000) - waypoints_next = init_waypoint.next(1000) - print(f"{init_waypoint.transform.location.x = }") - print(f"{init_waypoint.transform.location.y = }") - print(f"{init_waypoint.lane_id = }") - print(f"{init_waypoint.road_id = }") - print(f"{len(waypoints_lane) = }") - print(f"{len(waypoints_next) = }") - w_road = [] - w_lane = [] - for x in waypoints_next: - w_road.append(x.road_id) - w_lane.append(x.lane_id) - - counter_lanes = Counter(w_lane) - counter_road = Counter(w_road) - print(f"{counter_lanes = }") - print(f"{counter_road = }") - - self.setup_car_fix_pose(init_waypoint) - - ## --- Sensor collision - self.setup_col_sensor() + ## -- Always same init position in a circuit predefined + elif self.alternate_pose is False: + self.setup_car_pose(self.start_alternate_pose, init=self.init_pose_number) + + ## -- Same circuit, but random init positions + else: + # self.setup_car_alternate_pose(self.start_alternate_pose) + self.setup_car_pose(self.start_alternate_pose) ## --- Cameras - # self.camera_spectator = SensorManager( - # self.world, - # self.display_manager, - # "RGBCamera", - # carla.Transform(carla.Location(x=-5, z=2.8), carla.Rotation(yaw=+00)), - # self.car, - # {}, - # display_pos=[0, 0], - # ) - # self.camera_spectator_segmentated = SensorManager( - # self.world, - # self.display_manager, - # "SemanticCamera", - # carla.Transform(carla.Location(x=-5, z=2.8), carla.Rotation(yaw=+00)), - # self.car, - # {}, - # display_pos=[1, 0], - # ) - # self.sergio_camera_spectator = SensorManager( - # self.world, - # self.display_manager, - # "SemanticCameraSergio", - # carla.Transform(carla.Location(x=-5, z=2.8), carla.Rotation(yaw=+00)), - # self.car, - # {}, - # display_pos=[2, 0], - # ) - # self.front_camera = SensorManager( - # self.world, - # self.display_manager, - # "RGBCamera", - # carla.Transform(carla.Location(x=2, z=1), carla.Rotation(yaw=+00)), - # self.car, - # {}, - # display_pos=[0, 1], - # ) - # self.front_camera_segmentated = SensorManager( - # self.world, - # self.display_manager, - # "SemanticCamera", - # carla.Transform(carla.Location(x=2, z=1), carla.Rotation(yaw=+00)), - # self.car, - # {}, - # display_pos=[1, 1], - # ) + ## ---- RGB + # self.setup_rgb_camera() + self.setup_rgb_camera_weakref() + # self.setup_semantic_camera() + while self.sensor_camera_rgb is None: + time.sleep(0.01) - # self.sergio_front_camera = SensorManager( - # self.world, - # self.display_manager, - # "SemanticCameraSergio", - # carla.Transform(carla.Location(x=2, z=1), carla.Rotation(yaw=+00)), - # self.car, - # {}, - # display_pos=[2, 1], - # ) - # self.front_camera_mas_baja = SensorManager( - # self.world, - # self.display_manager, - # "RGBCamera", - # carla.Transform(carla.Location(x=2, z=0.5), carla.Rotation(yaw=+00)), - # self.car, - # {}, - # display_pos=[0, 2], - # ) - # self.front_camera_mas_baja_segmentated = SensorManager( - # self.world, - # self.display_manager, - # "SemanticCamera", - # carla.Transform(carla.Location(x=2, z=0.5), carla.Rotation(yaw=+00)), - # self.car, - # {}, - # display_pos=[1, 2], - # ) + ## ---- Red Mask + self.setup_red_mask_camera_weakref() + while self.sensor_camera_red_mask is None: + time.sleep(0.01) - # self.sergio_front_camera_mas_baja = SensorManager( - # self.world, - # self.display_manager, - # "SemanticCameraSergio", - # carla.Transform(carla.Location(x=2, z=0.5), carla.Rotation(yaw=+0)), - # self.car, - # {}, - # display_pos=[2, 2], - # ) + ## ---- LAneDetector + # self.setup_lane_detector_camera_weakref() + # while self.sensor_camera_lane_detector is None: + # time.sleep(0.01) - self.front_camera_1_5_bev = SensorManager( - self.world, - self.display_manager, - "BirdEyeView", - carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)), - self.car, - {}, - display_pos=[1, 1], - client=self.client, - ) - - self.front_camera_1_5 = SensorManager( - self.world, - self.display_manager, - "RGBCamera", - carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)), - self.car, - {}, - display_pos=[0, 0], - ) - - self.front_camera_1_5_segmentated = SensorManager( - self.world, - self.display_manager, - "SemanticCamera", - carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)), - self.car, - {}, - display_pos=[0, 1], - ) - - self.front_camera_1_5_red_mask = SensorManager( - self.world, - self.display_manager, - "RedMask", - carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+0)), - self.car, - {}, - display_pos=[0, 2], - ) + ## --- Detectors Sensors + self.setup_lane_invasion_sensor_weakref() time.sleep(1) - self.episode_start = time.time() + + ## Autopilot + # self.car.set_autopilot(True) self.car.apply_control(carla.VehicleControl(throttle=0.0, brake=0.0)) + self.setup_spectator() + + ########### --- calculating STATES + mask = self.preprocess_image(self.front_red_mask_camera) + right_line_in_pixels = self.calculate_line_right(mask) + + pixels_in_state = mask.shape[1] / self.num_regions + states = [ + int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + ] + + return states - AutoCarlaUtils.show_image("image", self.front_camera_1_5.front_camera, 1) + def reset_one_point_between_right_left_lines(self): + ### --- stoping and destroying all actors + # if (self.col_sensor is not None and self.col_sensor.is_listening) or ( + # self.sensor_camera_rgb is not None and self.sensor_camera_rgb.is_listening + # ): + # if self.sensor_camera_rgb is not None or self.sensor_camera_rgb.is_listening: + if self.sensor_camera_rgb is not None: + # self.col_sensor.stop() + # self.sensor_camera_rgb.stop() + # self.sensor_camera_red_mask.stop() + # self.sensor_bev_camera.stop() + # self.sensor_camera_segmentation.stop() + # self.lane_sensor.stop() + # self.obstacle_sensor.stop() + self.destroy_all_actors_apply_batch() + + self.col_sensor = None + self.sensor_camera_rgb = None + self.sensor_camera_red_mask = None + self.front_camera_bev = None + self.front_camera_bev_mask = None + self.sensor_camera_segmentation = None + self.lane_sensor = None + self.obstacle_sensor = None + self.collision_hist = [] + self.lane_changing_hist = [] + self.obstacle_hist = [] + self.actor_list = [] - mask = self.preprocess_image( - self.front_camera_1_5_red_mask.front_camera_red_mask + ## --- Car + waypoints_town = self.world.get_map().generate_waypoints(5.0) + init_waypoint = waypoints_town[self.waypoints_init + 1] + filtered_waypoints = self.draw_waypoints( + waypoints_town, + self.waypoints_init, + self.waypoints_target, + self.waypoints_lane_id, + 2000, ) - AutoCarlaUtils.show_image("mask", mask, 1) + self.target_waypoint = filtered_waypoints[-1] - # Wait for world to get the vehicle actor - self.world.tick() + ## ---- random init position in the whole Town: actually for test functioning porposes + if self.random_pose: + self.setup_car_random_pose() + ## -- Always same init position in a circuit predefined + elif self.alternate_pose is False: + # self.setup_car_fix_pose(init_waypoint) + # self.setup_car_fix_pose(self.start_alternate_pose) + self.setup_car_pose(self.start_alternate_pose, init=self.init_pose_number) - world_snapshot = self.world.wait_for_tick() - actor_snapshot = world_snapshot.find(self.car.id) - # Set spectator at given transform (vehicle transform) - spectator.set_transform(actor_snapshot.get_transform()) + ## -- Same circuit, but random init positions + else: + # self.setup_car_alternate_pose(self.start_alternate_pose) + self.setup_car_pose(self.start_alternate_pose) + + """ + if self.waypoints_target is not None: + # waypoints = self.get_waypoints() + self.setup_car_fix_pose(init_waypoint) + + else: # TODO: hacer en el caso que se quiera poner el target con .next() + waypoints_lane = init_waypoint.next_until_lane_end(1000) + waypoints_next = init_waypoint.next(1000) + print(f"{init_waypoint.transform.location.x = }") + print(f"{init_waypoint.transform.location.y = }") + print(f"{init_waypoint.lane_id = }") + print(f"{init_waypoint.road_id = }") + print(f"{len(waypoints_lane) = }") + print(f"{len(waypoints_next) = }") + w_road = [] + w_lane = [] + for x in waypoints_next: + w_road.append(x.road_id) + w_lane.append(x.lane_id) + + counter_lanes = Counter(w_lane) + counter_road = Counter(w_road) + print(f"{counter_lanes = }") + print(f"{counter_road = }") + + self.setup_car_fix_pose(init_waypoint) + """ + ## --- Cameras + + ## ---- RGB + # self.setup_rgb_camera() + self.setup_rgb_camera_weakref() + # self.setup_semantic_camera() + while self.sensor_camera_rgb is None: + # print("1") + time.sleep(0.01) + + ## ---- Red Mask + self.setup_red_mask_camera_weakref() + # self.setup_red_mask_camera() + # self.setup_bev_camera() + while self.sensor_camera_red_mask is None: + time.sleep(0.01) + + ## ---- BEV camera + # self.setup_bev_camera_weakref() + # self.setup_bev_camera() + # while self.sensor_bev_camera is None: + # time.sleep(0.01) + + ## --- SEgmentation camera + # self.setup_segmentation_camera_weakref() + # self.setup_segmentation_camera() + # while self.sensor_camera_segmentation is None: + # time.sleep(0.01) + + ## --- Detectors Sensors + # self.setup_col_sensor_weakref() + # self.setup_col_sensor() + self.setup_lane_invasion_sensor_weakref() + # self.setup_lane_invasion_sensor() + # self.setup_obstacle_sensor_weakref() + # self.setup_obstacle_sensor() + + # AutoCarlaUtils.show_images( + # "main", self.front_rgb_camera, self.front_red_mask_camera, 600, 5 + # ) + + time.sleep(1) + + ## Autopilot + # self.car.set_autopilot(True) + self.car.apply_control(carla.VehicleControl(throttle=0.0, brake=0.0)) + # self.setup_spectator() + + ########### --- calculating STATES + mask = self.preprocess_image(self.front_red_mask_camera) + lane_centers_in_pixels, _, _ = self.calculate_lane_centers(mask) + ### errors: distance to center of image to lane center + image_center = mask.shape[1] // 2 + # errors = [ + # image_center - lane_centers_in_pixels[i] + # for i, _ in enumerate(lane_centers_in_pixels) + # ] + ### + # errors_normalized = [ + # float((image_center - errors[i]) / image_center) + # for i, _ in enumerate(errors)] + pixels_in_state = mask.shape[1] / self.num_regions + states = [ + int(value / pixels_in_state) + for _, value in enumerate(lane_centers_in_pixels) + ] ## -- states - ( - states, - distance_center, - distance_to_centr_normalized, - ) = self.calculate_states(mask) + # ( + # states, + # distance_center, + # distance_to_centr_normalized, + # ) = self.calculate_states(mask) - self.perfect_distance_pixels = distance_center - self.perfect_distance_normalized = distance_to_centr_normalized + # self.perfect_distance_pixels = distance_center + # self.perfect_distance_normalized = distance_to_centr_normalized return states #################################################### + # + # Reset Methods #################################################### + + def calculate_lane_centers_with_lane_detector(self, mask): + """ + using Lane Detector model + """ + lines = [mask[self.x_row[i], :] for i, _ in enumerate(self.x_row)] + index_left = [np.argmax(lines[x]) for x, _ in enumerate(lines)] + left_offset, right_offset = 20, 10 + index_left_plus_offset = [ + index_left[x] + left_offset if index_left[x] != 0 else 0 + for x, _ in enumerate(index_left) + ] + second_lines_from_left_right = [ + lines[x][index_left_plus_offset[x] :] for x, _ in enumerate(lines) + ] + ## ---------------- calculating index in second line + try: + index_right_no_offset = [ + np.argmax(second_lines_from_left_right[x]) + for x, _ in enumerate(second_lines_from_left_right) + ] + except: + secod_lines_from_left_right = [value for _, value in enumerate(self.x_row)] + index_right_no_offset = [ + np.argmax(second_lines_from_left_right[x]) + for x, _ in enumerate(second_lines_from_left_right) + ] + + index_right_plus_offsets = [ + index_right_no_offset[x] + left_offset + right_offset + for x, _ in enumerate(index_right_no_offset) + ] + + index_right = [ + right + left for right, left in zip(index_right_plus_offsets, index_left) + ] + + centers = [ + ((right - left) // 2) + left for right, left in zip(index_right, index_left) + ] + + return centers, index_left, index_right + + def calculate_line_right(self, mask): + """ + calculates distance from center to right line + This distance will be using as a error from center lane + """ + ## get total lines in every line point + lines = [mask[self.x_row[i], :] for i, _ in enumerate(self.x_row)] + + ### ----------------- from right to left + lines_inversed = [list(reversed(lines[x])) for x, _ in enumerate(lines)] + # print(f"{lines_inversed = }") + inv_index_right = [ + np.argmax(lines_inversed[x]) for x, _ in enumerate(lines_inversed) + ] + # print(f"{inv_index_right = }") + # offset = 10 + # inv_index_right_plus_offset = [ + # inv_index_right[x] + offset if inv_index_right[x] != 0 else 0 + # for x, _ in enumerate(inv_index_right) + # ] + # print(f"{inv_index_right = }") + # index_right = [ + # mask.shape[1] - inv_index_right_plus_offset[x] + # if inv_index_right_plus_offset[x] != 0 + # else 0 + # for x, _ in enumerate(inv_index_right_plus_offset) + # ] + index_right = [ + mask.shape[1] - inv_index_right[x] if inv_index_right[x] != 0 else 0 + for x, _ in enumerate(inv_index_right) + ] + + return index_right + + def calculate_lane_centers(self, mask): + """ + calculating LANE CENTRAL FROM RIGHT TO LEFT (using Sergio segmentation) + """ + + ## get total lines in every line point + lines = [mask[self.x_row[i], :] for i, _ in enumerate(self.x_row)] + + ### ----------------- from right to left + lines_inversed = [list(reversed(lines[x])) for x, _ in enumerate(lines)] + # print(f"{lines_inversed = }") + inv_index_right = [ + np.argmax(lines_inversed[x]) for x, _ in enumerate(lines_inversed) + ] + # print(f"{inv_index_right = }") + offset = 25 # 20 + inv_index_right_plus_offset = [ + inv_index_right[x] + offset if inv_index_right[x] != 0 else 0 + for x, _ in enumerate(inv_index_right) + ] + # print(f"{inv_index_right = }") + index_right = [ + mask.shape[1] - inv_index_right[x] if inv_index_right[x] != 0 else 0 + for x, _ in enumerate(inv_index_right) + ] + + ### --------------- now second line from rght to left + cropped_lines_inversed = [ + lines_inversed[x][inv_index_right_plus_offset[x] :] + for x, _ in enumerate(lines_inversed) + ] + ## ---------------- calculating index in second line + try: + second_inv_index_right = [ + np.argmax(cropped_lines_inversed[x]) + for x, _ in enumerate(cropped_lines_inversed) + ] + except: + # print(f"{cropped_lines_inversed =}") + # print(f"{len(x_row) =}") + cropped_lines_inversed = [value for _, value in enumerate(self.x_row)] + # cropped_lines_inversed = [0] + second_inv_index_right = [ + np.argmax(cropped_lines_inversed[x]) + for x, _ in enumerate(cropped_lines_inversed) + ] + # print(f"{second_inv_index_right =}") + + # now take off the offset + no_offset = offset - 5 + second_inv_index_right = [ + second_inv_index_right[x] + no_offset + for x, _ in enumerate(second_inv_index_right) + ] + + index_left = [ + mask.shape[1] - inv_index_right[x] - second_inv_index_right[x] + if second_inv_index_right[x] != 0 + else 0 + for x, _ in enumerate(second_inv_index_right) + ] + # print(f"{index_left =}") + + # print(f"{index_right = }") + + ### --- now from left to right + # print(f"{lines = }") + # index_left = [np.argmax(lines[x]) for x, _ in enumerate(lines)] + # print(f"{index_left = }") + # index_left = [index_left[x] + cte_left for x, _ in enumerate(index_left)] + # print(f"{index_left = }") + + ### --- calculate center of lane in every moment + # width = mask.shape[1] + # print(f"{width = }") + centers = [ + ((right - left) // 2) + left + # if (index_right[right] != 0) and (index_left[left] != 0) + # else 0 + for right, left in zip(index_right, index_left) + ] + # print(f"{centers = }") + + return centers, index_left, index_right + def calculate_states(self, mask): + """ + from right, search for red line + """ width = mask.shape[1] center_image = width // 2 ## get total lines in every line point @@ -304,9 +703,9 @@ def calculate_states(self, mask): width - inv_index_right[x] - center_image for x, _ in enumerate(inv_index_right) ] - ## normalized distances + ## normalized distances NO ABS distance_to_center_normalized = [ - abs(float((center_image - index_right[i]) / center_image)) + float((center_image - index_right[i]) / center_image) for i, _ in enumerate(index_right) ] pixels_in_state = mask.shape[1] / self.num_regions @@ -315,6 +714,9 @@ def calculate_states(self, mask): return states, distance_to_center, distance_to_center_normalized def preprocess_image(self, red_mask): + """ + image is cropping from top to middle + """ ## first, we cut the upper image height = red_mask.shape[0] image_middle_line = (height) // 2 @@ -323,12 +725,39 @@ def preprocess_image(self, red_mask): height = img_sliced.shape[0] ## -- convert to GRAY gray_mask = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2GRAY) - ## -- aplicamos mascara para convertir a BLANCOS Y NEGROS - _, white_mask = cv2.threshold(gray_mask, 10, 255, cv2.THRESH_BINARY) + ## -- apply mask to convert in Black and White + theshold = 50 + # _, white_mask = cv2.threshold(gray_mask, 10, 255, cv2.THRESH_BINARY) + _, white_mask = cv2.threshold(gray_mask, theshold, 255, cv2.THRESH_BINARY) return white_mask + def preprocess_image_lane_detector(self, image): + """ + image from lane detector + """ + ## first, we cut the upper image + height = image.shape[0] + image_middle_line = (height) // 2 + img_sliced = image[image_middle_line:] + + kernel = np.ones((5, 5), np.uint8) + img_erosion = cv2.erode(img_sliced, kernel, iterations=1) + hsv = cv2.cvtColor(img_erosion, cv2.COLOR_RGB2HSV) + gray_hsv = cv2.cvtColor(hsv, cv2.COLOR_BGR2GRAY) + _, gray_mask = cv2.threshold(gray_hsv, 200, 255, cv2.THRESH_BINARY) + + return gray_mask + + ######################################################## + # waypoints, car pose, sensors + # + ######################################################## + def draw_waypoints(self, spawn_points, init, target, lane_id, life_time): + """ + WArning: target always has to be the last waypoint, since it is the FINISH + """ filtered_waypoints = [] i = init for waypoint in spawn_points[init + 1 : target + 2]: @@ -357,26 +786,70 @@ def draw_waypoints(self, spawn_points, init, target, lane_id, life_time): return filtered_waypoints - def get_target_waypoint(self, target_waypoint, life_time): - """ - draw target point - """ - self.world.debug.draw_string( - target_waypoint.transform.location, - "O", - draw_shadow=False, - color=carla.Color(r=255, g=0, b=0), - life_time=life_time, - persistent_lines=True, + # @profile + def setup_car_pose(self, init_positions, init=None): + if init is None: + pose_init = np.random.randint(0, high=len(init_positions)) + else: + pose_init = init + + # print(f"{pose_init =}") + # print(f"{random_init = }") + # print(f"{self.start_alternate_pose = }") + # print(f"{self.start_alternate_pose[random_init][0] = }") + location = carla.Transform( + carla.Location( + x=self.start_alternate_pose[pose_init][0], + y=self.start_alternate_pose[pose_init][1], + z=self.start_alternate_pose[pose_init][2], + ), + carla.Rotation( + pitch=self.start_alternate_pose[pose_init][3], + yaw=self.start_alternate_pose[pose_init][4], + roll=self.start_alternate_pose[pose_init][5], + ), + ) + + self.car = self.world.spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(self.vehicle, location) + + self.actor_list.append(self.car) + time.sleep(1) + + def setup_car_alternate_pose(self, init_positions): + random_init = np.random.randint(0, high=len(init_positions)) + # print(f"{random_init = }") + # print(f"{self.start_alternate_pose = }") + # print(f"{self.start_alternate_pose[random_init][0] = }") + location = carla.Transform( + carla.Location( + x=self.start_alternate_pose[random_init][0], + y=self.start_alternate_pose[random_init][1], + z=self.start_alternate_pose[random_init][2], + ), + carla.Rotation( + pitch=self.start_alternate_pose[random_init][3], + yaw=self.start_alternate_pose[random_init][4], + roll=self.start_alternate_pose[random_init][5], + ), ) + self.car = self.world.spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(self.vehicle, location) + + self.actor_list.append(self.car) + time.sleep(1) + def setup_car_fix_pose(self, init): - car_bp = self.world.get_blueprint_library().filter("vehicle.*")[0] - # location = random.choice(self.world.get_map().get_spawn_points()) - # location = carla.Transform( - # carla.Location(x=-14.130021, y=69.766624, z=4), - # carla.Rotation(pitch=360.000000, yaw=0.073273, roll=0.000000), - # ) + """ + Town07, road 20, -1, init curves + (73.7, -10, 0.3, 0.0, -62.5, 0.0) + """ + + """ + # car_bp = self.world.get_blueprint_library().filter("vehicle.*")[0] location = carla.Transform( carla.Location( x=init.transform.location.x, @@ -389,746 +862,1665 @@ def setup_car_fix_pose(self, init): roll=init.transform.rotation.roll, ), ) + + print(f"{init.transform.location.x = }") + print(f"{init.transform.location.y = }") + print(f"{init.lane_id = }") + print(f"{init.road_id = }") + print(f"{init.s = }") + print(f"{init.id = }") + print(f"{init.lane_width = }") + print(f"{init.lane_change = }") + print(f"{init.lane_type = }") + print(f"{init.right_lane_marking = }") + """ + + location = carla.Transform( + carla.Location(x=73.7, y=-10, z=0.300000), + carla.Rotation(pitch=0.000000, yaw=-62.5, roll=0.000000), + ) - self.car = self.world.spawn_actor(car_bp, location) + self.car = self.world.spawn_actor(self.vehicle, location) while self.car is None: - self.car = self.world.spawn_actor(car_bp, location) + self.car = self.world.spawn_actor(self.vehicle, location) self.actor_list.append(self.car) time.sleep(1) def setup_car_random_pose(self): - car_bp = self.world.get_blueprint_library().filter("vehicle.*")[0] + # car_bp = self.world.get_blueprint_library().filter("vehicle.*")[0] location = random.choice(self.world.get_map().get_spawn_points()) - self.car = self.world.try_spawn_actor(car_bp, location) + self.car = self.world.try_spawn_actor(self.vehicle, location) while self.car is None: - self.car = self.world.try_spawn_actor(car_bp, location) + self.car = self.world.try_spawn_actor(self.vehicle, location) self.actor_list.append(self.car) time.sleep(1) - def setup_col_sensor(self): - colsensor = self.world.get_blueprint_library().find("sensor.other.collision") + ################## + # + # Detectors Sensors + ################## + + # @profile + def setup_col_sensor_weakref(self): + """weakref""" + # colsensor = self.world.get_blueprint_library().find("sensor.other.collision") transform = carla.Transform(carla.Location(x=2.5, z=0.7)) - self.colsensor = self.world.spawn_actor( - colsensor, transform, attach_to=self.car + self.col_sensor = self.world.spawn_actor( + self.colsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.col_sensor) + weak_self = weakref.ref(self) + self.col_sensor.listen( + lambda event: FollowLaneQlearnStaticWeatherNoTraffic._collision_data( + weak_self, event + ) ) - self.actor_list.append(self.colsensor) - self.colsensor.listen(lambda event: self.collision_data(event)) + # self.col_sensor.listen(self.collision_data) - def collision_data(self, event): - self.collision_hist.append(event) + @staticmethod + def _collision_data(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return - def destroy_all_actors(self): - for actor in self.actor_list[::-1]: - # for actor in self.actor_list: - actor.destroy() - # print(f"\nin self.destroy_all_actors(), actor : {actor}\n") + self.collision_hist.append(event) + actor_we_collide_against = event.other_actor + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + print( + f"you have crashed with {actor_we_collide_against.type_id} with impulse {impulse} and intensity {intensity}" + ) - # self.actor_list = [] - # .client.apply_batch( - # [carla.command.DestroyActor(x) for x in self.actor_list[::-1]] - # ) + def setup_col_sensor(self): + # colsensor = self.world.get_blueprint_library().find("sensor.other.collision") + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.col_sensor = self.world.spawn_actor( + self.colsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.col_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + self.col_sensor.listen(self.collision_data) - ################################################################################# - def step(self, action): - # print(f"=============== STEP ===================") + def collision_data(self, event): + self.collision_hist.append(event) + actor_we_collide_against = event.other_actor + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + print( + f"you have crashed with {actor_we_collide_against.type_id} with impulse {impulse} and intensity {intensity}" + ) + # self.actor_list.append(actor_we_collide_against) - ### -------- send action - params = self.control(action) - # print(f"params = {params}") + def setup_lane_invasion_sensor_weakref(self): + """weakref""" + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.lane_sensor = self.world.spawn_actor( + self.laneinvsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.lane_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + weak_self = weakref.ref(self) + self.lane_sensor.listen(self.lane_changing_data) + self.lane_sensor.listen( + lambda event: FollowLaneQlearnStaticWeatherNoTraffic._lane_changing( + weak_self, event + ) + ) - # params["pos"] = 270 - # center = 270 - # stados = random.randint(0, 4) - # stados = [stados] - # print(f"stados = {stados}") + @staticmethod + def _lane_changing(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + self.lane_changing_hist.append(event) + # print(f"you have changed the lane") - ## -- states - mask = self.preprocess_image( - self.front_camera_1_5_red_mask.front_camera_red_mask + def setup_lane_invasion_sensor(self): + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.lane_sensor = self.world.spawn_actor( + self.laneinvsensor, transform, attach_to=self.car ) + self.actor_list.append(self.lane_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + self.lane_sensor.listen(self.lane_changing_data) - AutoCarlaUtils.show_image("mask", mask, 1) + def lane_changing_data(self, event): + self.lane_changing_hist.append(event) + print(f"you have changed the lane") - ( - states, - distance_to_center, - distance_to_center_normalized, - ) = self.calculate_states(mask) - - # print(f"states:{states}\n") - AutoCarlaUtils.show_image_with_centrals( - "image", - self.front_camera_1_5.front_camera[mask.shape[0] :], - 1, - distance_to_center, - distance_to_center_normalized, - self.x_row, + def setup_obstacle_sensor_weakref(self): + """weakref""" + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.obstacle_sensor = self.world.spawn_actor( + self.obstsensor, transform, attach_to=self.car ) - - ## ------ calculate distance error and states - # print(f"{self.perfect_distance_normalized =}") - error = [ - abs( - self.perfect_distance_normalized[index] - - distance_to_center_normalized[index] + self.actor_list.append(self.obstacle_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + weak_self = weakref.ref(self) + # self.obstacle_sensor.listen(self.obstacle_data) + self.obstacle_sensor.listen( + lambda event: FollowLaneQlearnStaticWeatherNoTraffic._obstacle_sensor( + weak_self, event ) - for index, value in enumerate(self.x_row) - ] - counter_states = Counter(states) - states_16 = counter_states.get(16) - - ## -------- Rewards - reward = self.rewards_easy(error, params) + ) - ## ----- hacemos la salida - done = False - if states_16 is not None and ( - states_16 > (len(states) // 2) - ): # salimos porque no detecta linea a la derecha - done = True - if len(self.collision_hist) > 0: # te has chocado, baby - done = True + @staticmethod + def _obstacle_sensor(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + self.obstacle_hist.append(event) + print(f"you have found an obstacle") - # print_messages( - # "in step()", - # height=mask.shape[0], - # width=mask.shape[1], - # velocity=params["velocity"], - # steering_angle=params["steering_angle"], - # states=states, - # distance_to_center=distance_to_center, - # distance_to_center_normalized=distance_to_center_normalized, - # self_perfect_distance_pixels=self.perfect_distance_pixels, - # self_perfect_distance_normalized=self.perfect_distance_normalized, - # error=error, - # done=done, - # reward=reward, - # states_16=states_16, - # self_collision_hist=self.collision_hist, - # ) + def setup_obstacle_sensor(self): + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.obstacle_sensor = self.world.spawn_actor( + self.obstsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.obstacle_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + self.obstacle_sensor.listen(self.obstacle_data) + + def obstacle_data(self, event): + self.obstacle_hist.append(event) + print(f"you have found an obstacle") + + ################## + # + # spectator + ################## + def setup_spectator(self): + # self.spectator = self.world.get_spectator() + car_transfor = self.car.get_transform() + # world_snapshot = self.world.wait_for_tick() + # actor_snapshot = world_snapshot.find(self.car.id) + # Set spectator at given transform (vehicle transform) + # self.spectator.set_transform(actor_snapshot.get_transform()) + self.spectator.set_transform( + carla.Transform( + car_transfor.location + carla.Location(z=60), + carla.Rotation(pitch=-90, roll=-90), + ) + ) + # self.actor_list.append(self.spectator) - return states, reward, done, {} + ################## + # + # Cameras + ################## - def control(self, action): + # @profile + def setup_rgb_camera_weakref(self): + """weakref""" - steering_angle = 0 - if action == 0: - self.car.apply_control(carla.VehicleControl(throttle=1, steer=-0.2)) - steering_angle = -0.2 - elif action == 1: - self.car.apply_control(carla.VehicleControl(throttle=1, steer=0.0)) - steering_angle = 0 - elif action == 2: - self.car.apply_control(carla.VehicleControl(throttle=1, steer=0.2)) - steering_angle = 0.2 - # elif action == 3: - # self.vehicle.apply_control(carla.VehicleControl(throttle=0.2, steer=-0.4)) - # steering_angle = 0.4 - # elif action == 4: - # self.vehicle.apply_control(carla.VehicleControl(throttle=0.2, steer=0.4)) - # steering_angle = 0.4 - else: - print("error in action") - pass - params = {} + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_rgb = self.world.spawn_actor( + self.rgb_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_rgb) + # print(f"{len(self.actor_list) = }") + weak_self = weakref.ref(self) + # self.sensor_camera_rgb.listen(self.save_rgb_image) + self.sensor_camera_rgb.listen( + lambda event: FollowLaneQlearnStaticWeatherNoTraffic._rgb_image( + weak_self, event + ) + ) - v = self.car.get_velocity() - params["velocity"] = math.sqrt(v.x**2 + v.y**2 + v.z**2) + @staticmethod + def _rgb_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return - w = self.car.get_angular_velocity() - params["steering_angle"] = steering_angle - - return params - - def rewards_followlane_dist_v_angle(self, error, params): - # rewards = [] - # for i,_ in enumerate(error): - # if (error[i] < 0.2): - # rewards.append(10) - # elif (0.2 <= error[i] < 0.4): - # rewards.append(2) - # elif (0.4 <= error[i] < 0.9): - # rewards.append(1) - # else: - # rewards.append(0) - rewards = [0.1 / error[i] for i, _ in enumerate(error)] - function_reward = sum(rewards) / len(rewards) - function_reward += math.log10(params["velocity"]) - function_reward -= 1 / (math.exp(params["steering_angle"])) - - return function_reward - - def rewards_easy(self, error, params): - rewards = [] - for i, _ in enumerate(error): - if error[i] < 0.2: - rewards.append(10) - elif 0.2 <= error[i] < 0.4: - rewards.append(2) - elif 0.4 <= error[i] < 0.9: - rewards.append(1) - else: - rewards.append(-100) + image = np.array(image.raw_data) + image = image.reshape((480, 640, 4)) + # image = image.reshape((512, 1024, 4)) + image = image[:, :, :3] + # self._data_dict["image"] = image3 + self.front_rgb_camera = image + + def setup_rgb_camera(self): + # print("enter setup_rg_camera") + # rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # rgb_cam.set_attribute("image_size_x", f"{self.width}") + # rgb_cam.set_attribute("image_size_y", f"{self.height}") + # rgb_cam.set_attribute("fov", f"110") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_rgb = self.world.spawn_actor( + self.rgb_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_rgb) + # print(f"{len(self.actor_list) = }") + self.sensor_camera_rgb.listen(self.save_rgb_image) - function_reward = sum(rewards) / len(rewards) - function_reward += params["velocity"] * 0.5 - function_reward -= params["steering_angle"] * 1.02 + def save_rgb_image(self, data: carla.Image): + if not isinstance(data, carla.Image): + raise ValueError("Argument must be a carla.Image") + image = np.array(data.raw_data) + image = image.reshape((480, 640, 4)) + image = image[:, :, :3] + # self._data_dict["image"] = image3 + self.front_rgb_camera = image - return function_reward + def _save_rgb_image(self, image): + # t_start = self.timer.time() - def rewards_followlane_center_v_w(self): - """esta sin terminar""" - center = 0 - done = False - if 0.65 >= center > 0.25: - reward = 10 - elif (0.9 > center > 0.65) or (0.25 >= center > 0): - reward = 2 - elif 0 >= center > -0.9: - reward = 1 - else: - reward = -100 - done = True + image.convert(carla.ColorConverter.Raw) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] - return reward, done + # if self.display_man.render_enabled(): + # self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + # cv2.imshow("", array) + # cv2.waitKey(1) + self.front_rgb_camera = array + + ################## + # + # Red Mask Camera + ################## + def setup_red_mask_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_red_mask = self.world.spawn_actor( + self.red_mask_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_red_mask) + weak_self = weakref.ref(self) -########################################################################################################### -############################################################################################################ -############################################################################################################# -class OLD_FollowLaneQlearnStaticWeatherNoTraffic(FollowLaneEnv): - def __init__(self, **config): + # start_camera_red_mask = time.time() + self.sensor_camera_red_mask.listen( + lambda event: FollowLaneQlearnStaticWeatherNoTraffic._red_mask_semantic_image( + weak_self, event + ) + ) + # end_camera_red_mask = time.time() + # print( + # f"\n====> sensor_camera_red_mask time processing: {end_camera_red_mask - start_camera_red_mask = }" + # ) - print(f"in FollowLaneQlearnStaticWeatherNoTraffic -> launching FollowLaneEnv\n") - ###### init F1env - FollowLaneEnv.__init__(self, **config) - ###### init class variables - print(f"leaving FollowLaneEnv\n ") - print(f"launching FollowLaneCarlaConfig\n ") - FollowLaneCarlaConfig.__init__(self, **config) + # self.sensor_camera_red_mask.listen(self.save_red_mask_semantic_image) - self.client = carla.Client( - config["carla_server"], - config["carla_client"], - ) - self.client.set_timeout(5.0) - self.world = self.client.get_world() + @staticmethod + def _red_mask_semantic_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] - settings = self.world.get_settings() - settings.fixed_delta_seconds = 0.05 + hsv_nemo = cv2.cvtColor(array, cv2.COLOR_RGB2HSV) - if config["sync"]: - settings.synchronous_mode = True + if ( + self.world.get_map().name == "Carla/Maps/Town07" + or self.world.get_map().name == "Carla/Maps/Town04" + or self.world.get_map().name == "Carla/Maps/Town07_Opt" + or self.world.get_map().name == "Carla/Maps/Town04_Opt" + ): + light_sidewalk = (42, 200, 233) + dark_sidewalk = (44, 202, 235) else: - print(f"activo async") - settings.synchronous_mode = False + light_sidewalk = (151, 217, 243) + dark_sidewalk = (153, 219, 245) - self.world.apply_settings(settings) + light_pavement = (149, 127, 127) + dark_pavement = (151, 129, 129) - self.camera = None - self.vehicle = None - self.display = None - self.image = None + mask_sidewalk = cv2.inRange(hsv_nemo, light_sidewalk, dark_sidewalk) + # result_sidewalk = cv2.bitwise_and(array, array, mask=mask_sidewalk) - self.timer = CustomTimer() - self.time_processing = 0.0 - self.tics_processing = 0 + mask_pavement = cv2.inRange(hsv_nemo, light_pavement, dark_pavement) + # result_pavement = cv2.bitwise_and(array, array, mask=mask_pavement) - self.image_dict = {} + # Adjust according to your adjacency requirement. + kernel = np.ones((3, 3), dtype=np.uint8) - self.blueprint_library = self.world.get_blueprint_library() - self.model_3 = self.blueprint_library.filter("model3")[0] - # car_bp = self.blueprint_library.filter("vehicle.*")[0] + # Dilating masks to expand boundary. + color1_mask = cv2.dilate(mask_sidewalk, kernel, iterations=1) + color2_mask = cv2.dilate(mask_pavement, kernel, iterations=1) - ## --- Pygame Display - pygame.init() - pygame.font.init() - self.gameDisplay = pygame.display.set_mode( - (1500, 800), pygame.HWSURFACE | pygame.DOUBLEBUF - ) - self.clock = pygame.time.Clock() - self.surface = None + # Required points now will have both color's mask val as 255. + common = cv2.bitwise_and(color1_mask, color2_mask) + SOME_THRESHOLD = 0 - def __del__(self): - print("__del__ called") + # Common is binary np.uint8 image, min = 0, max = 255. + # SOME_THRESHOLD can be anything within the above range. (not needed though) + # Extract/Use it in whatever way you want it. + intersection_points = np.where(common > SOME_THRESHOLD) - def reset(self): - """ - reset for - - Algorithm: Q-learn - - State: Simplified perception - - tasks: FollowLane - """ + # Say you want these points in a list form, then you can do this. + pts_list = [[r, c] for r, c in zip(*intersection_points)] + # print(pts_list) - self.collision_hist = [] - self.actor_list = [] - self.image_dict = {} + # for x, y in pts_list: + # image_2[x][y] = (255, 0, 0) - ## --- CAR - self.setup_car() - """ - self.transform = random.choice(self.world.get_map().get_spawn_points()) - self.vehicle = self.world.spawn_actor(self.model_3, self.transform) - while self.vehicle is None: - self.vehicle = self.world.spawn_actor(self.model_3, self.transform) - self.actor_list.append(self.vehicle) - time.sleep(2.0) - """ + # red_line_mask = np.zeros((400, 500, 3), dtype=np.uint8) + red_line_mask = np.zeros((image.height, image.width, 3), dtype=np.uint8) - ## --- CAMERA FRONT - self.rgb_cam = self.blueprint_library.find("sensor.camera.rgb") - self.rgb_cam.set_attribute("image_size_x", "640") - self.rgb_cam.set_attribute("image_size_y", "480") - self.rgb_cam.set_attribute("fov", "110") - transform = carla.Transform( - carla.Location(x=2.5, z=0.7), carla.Rotation(yaw=+00) - ) - self.sensor_front_camera = self.world.spawn_actor( - self.rgb_cam, transform, attach_to=self.vehicle - ) - ## In case of problem in getting rid off sensors #### - ## We need to pass the lambda a weak reference to self to avoid circular - ## reference. - # weak_self = weakref.ref(self) - # self.sensor_front_camera.listen( - # lambda data: FollowLaneQlearnStaticWeatherNoTraffic._weak_process_image( - # weak_self, data - # ) + for x, y in pts_list: + red_line_mask[x][y] = (255, 0, 0) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + red_line_mask = cv2.cvtColor(red_line_mask, cv2.COLOR_BGR2RGB) + self.front_red_mask_camera = red_line_mask + + def setup_red_mask_camera(self): + # self.red_mask_cam = self.world.get_blueprint_library().find( + # "sensor.camera.semantic_segmentation" # ) - self.sensor_front_camera.listen(lambda data: self.process_image(data)) - while self.sensor_front_camera is None: - time.sleep(0.01) - self.actor_list.append(self.sensor_front_camera) - self.vehicle.apply_control(carla.VehicleControl(throttle=0.0, brake=0.0)) - time.sleep(2.0) - - ## --- Collision Sensor - colsensor = self.blueprint_library.find("sensor.other.collision") - self.colsensor = self.world.spawn_actor( - colsensor, transform, attach_to=self.vehicle - ) - self.actor_list.append(self.colsensor) - self.colsensor.listen(lambda event: self.collision_data(event)) - - self.episode_start = time.time() - self.vehicle.apply_control(carla.VehicleControl(throttle=0.0, brake=0.0)) - - #### VAMOs a enganar al step - stados = random.randint(0, 16) - stados = [stados] - # print(f"stados = {stados}") - return stados - - # return self.front_camera - - def setup_car(self): - self.transform = random.choice(self.world.get_map().get_spawn_points()) - self.vehicle = self.world.spawn_actor(self.model_3, self.transform) - while self.vehicle is None: - self.vehicle = self.world.spawn_actor(self.model_3, self.transform) - self.actor_list.append(self.vehicle) - time.sleep(2.0) - - def process_image(self, image): - """Convert a CARLA raw image to a BGRA numpy array.""" - print(f"--- reading image ------------------") - if not isinstance(image, carla.Image): - raise ValueError("Argument must be a carla.Image") - t_start = self.timer.time() + # self.red_mask_cam.set_attribute("image_size_x", f"{self.width}") + # self.red_mask_cam.set_attribute("image_size_y", f"{self.height}") - image.convert(carla.ColorConverter.Raw) + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_red_mask = self.world.spawn_actor( + self.red_mask_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_red_mask) + self.sensor_camera_red_mask.listen(self.save_red_mask_semantic_image) + + def save_red_mask_semantic_image(self, image): + # t_start = self.timer.time() + # print(f"en red_mask calbback") + image.convert(carla.ColorConverter.CityScapesPalette) array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) array = np.reshape(array, (image.height, image.width, 4)) array = array[:, :, :3] array = array[:, :, ::-1] - self.image_dict["image"] = array - # if self.display_manager.render_enabled(): - self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + hsv_nemo = cv2.cvtColor(array, cv2.COLOR_RGB2HSV) - t_end = self.timer.time() - self.time_processing += t_end - t_start - self.tics_processing += 1 - if self.surface is not None: - # offset = self.display_manager.get_display_offset([0, 0]) - self.gameDisplay.blit(self.surface, (640, 480)) - self.clock.tick(60) + if ( + self.world.get_map().name == "Carla/Maps/Town07" + or self.world.get_map().name == "Carla/Maps/Town04" + ): + light_sidewalk = (42, 200, 233) + dark_sidewalk = (44, 202, 235) + else: + light_sidewalk = (151, 217, 243) + dark_sidewalk = (153, 219, 245) - # print(f"self.image_dict = {self.image_dict}") - # cv2.imshow("", array) - # cv2.waitKey(1) - # time.sleep(0.1) - self.front_camera = array + light_pavement = (149, 127, 127) + dark_pavement = (151, 129, 129) - """ - def render(self): - if self.surface is not None: - offset = self.display_manager.get_display_offset([0, 0]) - self.display_manager.display.blit(self.surface, offset) - """ + mask_sidewalk = cv2.inRange(hsv_nemo, light_sidewalk, dark_sidewalk) + # result_sidewalk = cv2.bitwise_and(array, array, mask=mask_sidewalk) + + mask_pavement = cv2.inRange(hsv_nemo, light_pavement, dark_pavement) + # result_pavement = cv2.bitwise_and(array, array, mask=mask_pavement) + + # Adjust according to your adjacency requirement. + kernel = np.ones((3, 3), dtype=np.uint8) + + # Dilating masks to expand boundary. + color1_mask = cv2.dilate(mask_sidewalk, kernel, iterations=1) + color2_mask = cv2.dilate(mask_pavement, kernel, iterations=1) + + # Required points now will have both color's mask val as 255. + common = cv2.bitwise_and(color1_mask, color2_mask) + SOME_THRESHOLD = 0 + + # Common is binary np.uint8 image, min = 0, max = 255. + # SOME_THRESHOLD can be anything within the above range. (not needed though) + # Extract/Use it in whatever way you want it. + intersection_points = np.where(common > SOME_THRESHOLD) + + # Say you want these points in a list form, then you can do this. + pts_list = [[r, c] for r, c in zip(*intersection_points)] + # print(pts_list) + + # for x, y in pts_list: + # image_2[x][y] = (255, 0, 0) + + # red_line_mask = np.zeros((400, 500, 3), dtype=np.uint8) + red_line_mask = np.zeros((image.height, image.width, 3), dtype=np.uint8) + + for x, y in pts_list: + red_line_mask[x][y] = (255, 0, 0) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + red_line_mask = cv2.cvtColor(red_line_mask, cv2.COLOR_BGR2RGB) + self.front_red_mask_camera = red_line_mask + + ################## + # + # BEV + ################## + def setup_bev_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_bev_camera = self.world.spawn_actor( + self.bev_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_bev_camera) + # print(f"{len(self.actor_list) = }") + weak_self = weakref.ref(self) + # self.sensor_bev_camera.listen(self.save_bev_image) + self.sensor_bev_camera.listen( + lambda event: FollowLaneQlearnStaticWeatherNoTraffic._bev_image( + weak_self, event + ) + ) @staticmethod - def _weak_process_image(weak_self, image): - """Convert a CARLA raw image to a BGRA numpy array.""" + def _bev_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + + car_bp = self.world.get_actors().filter("vehicle.*")[0] + # car_bp = self.vehicle + birdview = self.birdview_producer.produce( + agent_vehicle=car_bp # carla.Actor (spawned vehicle) + ) + image = BirdViewProducer.as_rgb(birdview) + hsv_nemo = cv2.cvtColor(image, cv2.COLOR_RGB2HSV) + + # Extract central line + light_center_line = (17, 134, 232) + dark_center_line = (19, 136, 234) + mask_center_line = cv2.inRange(hsv_nemo, light_center_line, dark_center_line) + result = cv2.bitwise_and(hsv_nemo, hsv_nemo, mask=mask_center_line) + + # image = np.rot90(image) + image = np.array(image) + result = np.array(result) + if image.shape[0] != image.shape[1]: + if image.shape[0] > image.shape[1]: + difference = image.shape[0] - image.shape[1] + extra_left, extra_right = int(difference / 2), int(difference / 2) + extra_top, extra_bottom = 0, 0 + else: + difference = image.shape[1] - image.shape[0] + extra_left, extra_right = 0, 0 + extra_top, extra_bottom = int(difference / 2), int(difference / 2) + image = np.pad( + image, + ((extra_top, extra_bottom), (extra_left, extra_right), (0, 0)), + mode="constant", + constant_values=0, + ) + image = np.pad( + image, + ((100, 100), (50, 50), (0, 0)), + mode="constant", + constant_values=0, + ) + + self.front_camera_bev = image + self.front_camera_bev_mask = result + + def setup_bev_camera(self): + # print("enter setup_rg_camera") + # rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # rgb_cam.set_attribute("image_size_x", f"{self.width}") + # rgb_cam.set_attribute("image_size_y", f"{self.height}") + # rgb_cam.set_attribute("fov", f"110") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_bev_camera = self.world.spawn_actor( + self.bev_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_bev_camera) + # print(f"{len(self.actor_list) = }") + self.sensor_bev_camera.listen(self.save_bev_image) + + def save_bev_image(self, image): + car_bp = self.world.get_actors().filter("vehicle.*")[0] + # car_bp = self.vehicle + birdview = self.birdview_producer.produce( + agent_vehicle=car_bp # carla.Actor (spawned vehicle) + ) + image = BirdViewProducer.as_rgb(birdview) + hsv_nemo = cv2.cvtColor(image, cv2.COLOR_RGB2HSV) + + # Extract central line + light_center_line = (17, 134, 232) + dark_center_line = (19, 136, 234) + mask_center_line = cv2.inRange(hsv_nemo, light_center_line, dark_center_line) + result = cv2.bitwise_and(hsv_nemo, hsv_nemo, mask=mask_center_line) + + # image = np.rot90(image) + image = np.array(image) + result = np.array(result) + if image.shape[0] != image.shape[1]: + if image.shape[0] > image.shape[1]: + difference = image.shape[0] - image.shape[1] + extra_left, extra_right = int(difference / 2), int(difference / 2) + extra_top, extra_bottom = 0, 0 + else: + difference = image.shape[1] - image.shape[0] + extra_left, extra_right = 0, 0 + extra_top, extra_bottom = int(difference / 2), int(difference / 2) + image = np.pad( + image, + ((extra_top, extra_bottom), (extra_left, extra_right), (0, 0)), + mode="constant", + constant_values=0, + ) + image = np.pad( + image, + ((100, 100), (50, 50), (0, 0)), + mode="constant", + constant_values=0, + ) + + self.front_camera_bev = image + self.front_camera_bev_mask = result + + ################## + # + # Segmentation Camera + ################## + def setup_segmentation_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_segmentation = self.world.spawn_actor( + self.segm_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_segmentation) + # print(f"{len(self.actor_list) = }") + weak_self = weakref.ref(self) + # self.sensor_camera_segmentation.listen(self.save_segmentation_image) + self.sensor_camera_segmentation.listen( + lambda event: FollowLaneQlearnStaticWeatherNoTraffic._segmentation_image( + weak_self, event + ) + ) + + @staticmethod + def _segmentation_image(weak_self, image): + """weakref""" self = weak_self() if not self: return - if not isinstance(image, carla.Image): - raise ValueError("Argument must be a carla.Image") + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] - image = np.array(image.raw_data) - image2 = image.reshape((480, 640, 4)) - image3 = image2[:, :, :3] - self.image_dict["image"] = image3 - # print(f"self.image_dict = {self.image_dict}") - cv2.imshow("", image3) - cv2.waitKey(1) - time.sleep(0.1) - self.front_camera = image3 + self.segmentation_cam = array - def collision_data(self, event): - self.collision_hist.append(event) + def setup_segmentation_camera(self): + # print("enter setup_rg_camera") + # rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # rgb_cam.set_attribute("image_size_x", f"{self.width}") + # rgb_cam.set_attribute("image_size_y", f"{self.height}") + # rgb_cam.set_attribute("fov", f"110") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_segmentation = self.world.spawn_actor( + self.segm_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_segmentation) + # print(f"{len(self.actor_list) = }") + self.sensor_camera_segmentation.listen(self.save_segmentation_image) + + def save_segmentation_image(self, image: carla.Image): + # t_start = self.timer.time() + + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + self.segmentation_cam = array + # if self.display_man.render_enabled(): + # self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + ################## + # + # Destroyers + ################## + + def destroy_all_actors_apply_batch(self): + print("\ndestroying %d actors with apply_batch method" % len(self.actor_list)) + self.client.apply_batch( + [carla.command.DestroyActor(x) for x in self.actor_list[::-1]] + ) def destroy_all_actors(self): + print("\ndestroying %d actors with destroy() method" % len(self.actor_list)) for actor in self.actor_list[::-1]: actor.destroy() - ################################################# - ################################################# + ################################################################################### + # + # Step + ################################################################################### + """ def step(self, action): + if self.states_entry == "one_between_right_left_lines": + # print("entro en one_point") + return self.step_one_point_between_right_left_lines(self, action) - # print(f"entramos en step()") - ### -------- send action - params = self.control(action) - # print(f"params = {params}") + elif self.states_entry == "one_only_right_line": + return self.step_one_point_only_right_line(self, action) + """ - ### -------- State get center lane - # weak_self = weakref.ref(self) - # self.sensor_front_camera.listen( - # lambda data: FollowLaneQlearnStaticWeatherNoTraffic.process_image_weak( - # weak_self, data - # ) - # ) - # self.sensor_front_camera.listen(lambda data: self.process_image(data)) - # time.sleep(2.0) - # while self.sensor_front_camera is None: - # time.sleep(0.01) - # print(f"entro") + # def step_one_point_only_right_line(self, action): + def _____step(self, action): + self.control_only_right(action) - # params["pos"] = 270 - center = 270 - stados = random.randint(0, 4) - stados = [stados] - # print(f"stados = {stados}") - ## -------- Rewards - reward, done = self.rewards_followlane_centerline(center) + ########### --- calculating STATES + # start_camera_red_mask = time.time() + mask = self.preprocess_image(self.front_red_mask_camera) - return stados, reward, done, {} + ### errors: distance to center of image to lane center + image_center = mask.shape[1] // 2 - def control(self, action): + ########### --- Calculating center ONLY with right line + # calculate only with right line + right_line_in_pixels = self.calculate_line_right(mask) + # calculate dist with ONLY right line + dist = [ + right_line_in_pixels[i] - image_center + for i, _ in enumerate(right_line_in_pixels) + ] + dist_normalized = [ + float((right_line_in_pixels[i] - image_center) / image_center) + for i, _ in enumerate(right_line_in_pixels) + ] - steering_angle = 0 - if action == 0: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.32, steer=-0.2)) - steering_angle = 0.2 - elif action == 1: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.7, steer=0.0)) - steering_angle = 0 - elif action == 2: - self.vehicle.apply_control(carla.VehicleControl(throttle=0.32, steer=0.2)) - steering_angle = 0.2 - # elif action == 3: - # self.vehicle.apply_control(carla.VehicleControl(throttle=0.2, steer=-0.4)) - # steering_angle = 0.4 - # elif action == 4: - # self.vehicle.apply_control(carla.VehicleControl(throttle=0.2, steer=0.4)) - # steering_angle = 0.4 - else: - print("error en action") - pass - params = {} + ground_truth_normalized_values = [ + correct_normalized_distance[value] for i, value in enumerate(self.x_row) + ] + ground_truth_pixel_values = [ + correct_pixel_distance[value] for i, value in enumerate(self.x_row) + ] + error_normalized = [ + ground_truth_normalized_values[index] - abs(dist_normalized[index]) + for index, _ in enumerate(self.x_row) + ] + error_pixels = [ + right_line_in_pixels[index] - ground_truth_pixel_values[index] + for index, _ in enumerate(self.x_row) + ] - v = self.vehicle.get_velocity() - params["velocity"] = math.sqrt(v.x**2 + v.y**2 + v.z**2) + ### States + pixels_in_state = mask.shape[1] / self.num_regions + states = [ + int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + ] - w = self.vehicle.get_angular_velocity() - params["steering_angle"] = steering_angle + counter_states = Counter(states) + states_16 = counter_states.get(self.num_regions) + states_0 = counter_states.get(0) - return params + # print(f"{lane_centers_in_pixels = }") + AutoCarlaUtils.show_image_with_center_point( + "mask2", + mask, + 1, + error_pixels, + # right_line_in_pixels, + dist_normalized, + states, + self.x_row, + 600, + 10, + ) - def rewards_followlane_centerline(self, center): """ - works perfectly - rewards in function of center of Line + AutoCarlaUtils.show_image_with_three_points( + "mask3", + mask, + 1, + lane_centers_in_pixels, + # right_line_in_pixels, + dist_normalized, + states, + self.x_row, + 600, + 500, + left_points_pixels, + right_points_pixels, + ) """ + AutoCarlaUtils.show_image_with_center_point( + "front RGB", + self.front_rgb_camera[(self.front_rgb_camera.shape[0] // 2) :], + 1, + error_pixels, + # right_line_in_pixels, + dist_normalized, + states, + self.x_row, + 1250, + 10, + ) + + # error = [ + # abs( + # self.perfect_distance_normalized[index] + # - distance_to_center_normalized[index] + # ) + # for index, value in enumerate(self.x_row) + # ] + + ## -------- Ending Step()... done = False - if 0.65 >= center > 0.25: - reward = 10 - elif (0.9 > center > 0.65) or (0.25 >= center > 0): - reward = 2 - elif 0 >= center > -0.9: - reward = 1 - else: + ## -------- Rewards + # reward, done = self.autocarlarewards.rewards_right_line( + # dist_normalized, self.params + # ) + reward, done = self.autocarlarewards.rewards_right_line( + error_normalized, self.params + ) + # reward, done = self.autocarlarewards.rewards_right_line_gazebo( + # dist_normalized, self.params + # ) + + ## -------- ... or Finish by... + if (states_16 is not None and (states_16 >= len(states))) or ( + states_0 is not None and (states_0 >= len(states)) + ): # not red right line + print(f"no lines detected") + done = True reward = -100 + if len(self.collision_hist) > 0: # crashed you, baby done = True + reward = -100 + print(f"crash") + + self.is_finish, self.dist_to_finish = AutoCarlaUtils.finish_fix_number_target( + self.params["location"], + self.finish_alternate_pose, + self.finish_pose_number, + self.max_target_waypoint_distance, + ) + if self.is_finish: + print(f"Finish!!!!") + done = True + reward = 100 + + # if len(self.lane_changing_hist) > 1: # you leave the lane + # done = True + # reward = -100 + # print(f"out of lane") + + render_params( + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + steering_angle=self.params["steering_angle"], + # Steering_angle=self.params["Steering_angle"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + height=self.params["height"], + action=action, + speed_kmh=self.params["speed"], + acceleration_ms2=self.params["Acceleration"], + _="------------------------", + states=states, + right_line_in_pixels=right_line_in_pixels, + # image_center=image_center, + # right_line_in_pixels=right_line_in_pixels, + dist_to_center=dist, + dist_normalized=dist_normalized, + error_normalized=error_normalized, + error_pixels=error_pixels, + reward=reward, + # done=done, + distance_to_finish=self.dist_to_finish, + num_self_lane_change_hist=len(self.lane_changing_hist), + # states_0=states_0, + # states_16=states_16, + # num_self_collision_hist=len(self.collision_hist), + # num_self_obstacle_hist=len(self.obstacle_hist), + ) + + print_messages( + "in step()", + height=mask.shape[0], + width=mask.shape[1], + action=action, + velocity=self.params["speed"], + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + _="------------------------", + states=states, + right_line_in_pixels=right_line_in_pixels, + distance_to_center=dist, + distance_to_center_normalized=dist_normalized, + # self_perfect_distance_pixels=self.perfect_distance_pixels, + # self_perfect_distance_normalized=self.perfect_distance_normalized, + error_normalized=error_normalized, + error_pixels=error_pixels, + done=done, + reward=reward, + states_16=states_16, + states_0=states_0, + self_collision_hist=self.collision_hist, + ) + + return states, reward, done, {} + + ######################################################################### + + @staticmethod + def get_prediction(model, img_array): + with torch.no_grad(): + image_tensor = img_array.transpose(2, 0, 1).astype("float32") / 255 + x_tensor = torch.from_numpy(image_tensor).to("cuda").unsqueeze(0) + model_output = torch.softmax(model.forward(x_tensor), dim=1).cpu().numpy() + return model_output + + @staticmethod + def lane_detection_overlay(image, left_mask, right_mask): + res = np.copy(image) + # We show only points with probability higher than 0.5 + res[left_mask > 0.07, :] = [255, 0, 0] + res[right_mask > 0.07, :] = [0, 0, 255] - return reward, done + cv2.erode(left_mask, (7, 7), 4) + cv2.dilate(left_mask, (7, 7), 4) - ############################################################################################################ + return res - def AAAAA_reset(self): + ######################################################################### + + def step_lane_detector(self, action): + # def step(self, action): """ - reset for - - Algorithm: Q-learn - - State: Simplified perception - - tasks: FollowLane + lane detector """ - if len(self.display_manager.get_sensor_list()) > 0: - print(f"destruyendo sensors_list[]") - print( - f"len self.display_manager.sensor_list = {len(self.display_manager.sensor_list)}" - ) - self.display_manager.destroy() + self.control(action) - # if len(self.actor_list) > 0: - # print(f"destruyendo actors_list[]") - # for actor in self.actor_list: - # actor.destroy() + """ + ## take RGB image and apply lane detector + # self.front_rgb_camera #aqui tengo la imagen + back, left, right = self.lanedetector.get_prediction( + self.lanedetector.model, self.front_rgb_camera + )[0] + image_rgb_lanedetector = self.lanedetector.lane_detection_overlay(self.front_rgb_camera, left, right) + mask = self.preprocess_image_lane_detector(image_rgb_lanedetector) + """ + # model = torch.load('models/fastai_torch_lane_detector_model.pth') + # model.eval() + # back, left, right = self.get_prediction(self.model, self.front_rgb_camera)[0] + # image_rgb_lanedetector = self.lane_detection_overlay( + # self.front_rgb_camera, left, right + # ) - self.client.apply_batch( - [carla.command.DestroyActor(x) for x in self.actor_list] + image_rgb_lanedetector, left_mask, right_mask = self._detector.detect( + self.front_rgb_camera ) - self.collision_hist = [] - self.actor_list = [] - print(f"----1. in reset() len(self.actor_list) = {len(self.actor_list)}") - # self.display_manager.actor_list = [] - time.sleep(1) + mask = self.preprocess_image_lane_detector(image_rgb_lanedetector) - ## ----------------------------------------------- COCHE - car_bp = self.world.get_blueprint_library().filter("vehicle.*")[0] - location = random.choice(self.world.get_map().get_spawn_points()) - self.car = self.world.try_spawn_actor(car_bp, location) - while self.car is None: - # print(f"entro here {datetime.now()}") - self.car = self.world.try_spawn_actor(car_bp, location) - self.actor_list.append(self.car) - ## ----------------------------------------------- CAMERA FRONT - self.rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") - self.rgb_cam.set_attribute("image_size_x", f"640") - self.rgb_cam.set_attribute("image_size_y", f"480") - self.rgb_cam.set_attribute("fov", f"110") - transform = carla.Transform( - carla.Location(x=2.5, z=0.7), carla.Rotation(yaw=+00) + # plt.imshow(self.lane_detection_overlay(self.front_rgb_camera, left, right)) + # plt.show() + + ########### --- calculating STATES + # start_camera_red_mask = time.time() + # mask = self.preprocess_image(self.front_red_mask_camera) + + ########### --- Calculating center of 2 lines + ( + lane_centers_in_pixels, + left_points_pixels, + right_points_pixels, + ) = self.calculate_lane_centers(mask) + + ### errors: distance to center of image to lane center + image_center = mask.shape[1] // 2 + + # calculate dist from center in pixels + dist = [ + image_center - lane_centers_in_pixels[i] + for i, _ in enumerate(lane_centers_in_pixels) + ] + + dist_normalized = [ + float((image_center - abs(dist[i])) / image_center) + for i, _ in enumerate(dist) + ] + + pixels_in_state = mask.shape[1] / self.num_regions + states = [ + int(value / pixels_in_state) + for _, value in enumerate(lane_centers_in_pixels) + ] + + counter_states = Counter(states) + states_16 = counter_states.get(self.num_regions) + states_0 = counter_states.get(0) + + # print(f"{lane_centers_in_pixels = }") + AutoCarlaUtils.show_image_with_center_point( + "mask", + mask, + 1, + lane_centers_in_pixels, + # right_line_in_pixels, + dist_normalized, + states, + self.x_row, + 600, + 10, ) - self.front_camera = self.world.spawn_actor( - self.rgb_cam, transform, attach_to=self.car + + AutoCarlaUtils.show_image_with_three_points( + "LaneDetector RGB", + image_rgb_lanedetector[(image_rgb_lanedetector.shape[0] // 2) :], + 1, + lane_centers_in_pixels, + # right_line_in_pixels, + dist_normalized, + states, + self.x_row, + 600, + 500, + left_points_pixels, + right_points_pixels, ) - self.actor_list.append(self.front_camera) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - # weak_self = weakref.ref(self) - # self.front_camera.listen( - # lambda data: FollowLaneQlearnStaticWeatherNoTraffic.process_img( - # weak_self, data - # ) - # ) - self.front_camera.listen(lambda data: self.process_img(data)) - while self.front_camera is None: - time.sleep(0.01) - ## ----------------------------------------------- CAMERA FRONT - camera = SensorManager( - # SensorManager( - self.world, - self.display_manager, - "RGBCamera", - carla.Transform(carla.Location(x=-4, z=2.4), carla.Rotation(yaw=+00)), - self.car, - {}, - display_pos=[0, 0], - ) - # self.actor_list.append(camera.actor_list[0]) - # --------------- actuator - self.car.set_autopilot(True) - - for actor in self.actor_list: - print(f"in reset - actor in self.actor_list: {actor} \n") - - for actor in self.display_manager.sensor_list: - print(f"in reset - actor in self.display_manager.sensor_list: {actor} \n") - - # for actor in sensor_manager.actor_list: - # print(f"in reset - actor: {actor} \n") - # for actor in self.display_manager.actor_list: - # print(f"in reset - actor in self.display_manager.actor_list: {actor}\n") - - return self.front_camera - - def _____reset(self): - """ - esta funcionando pero no mata los sensores de SensorManager - reset for - - Algorithm: Q-learn - - State: Simplified perception - - tasks: FollowLane - """ + AutoCarlaUtils.show_image_with_center_point( + "front RGB", + self.front_rgb_camera[(self.front_rgb_camera.shape[0] // 2) :], + 1, + lane_centers_in_pixels, + # right_line_in_pixels, + dist_normalized, + states, + self.x_row, + 1250, + 10, + ) - print(f"\nin reset()\n") - # if len(self.bsc.actor_list) > 0: - # print(f"destruyendo actors_list[]") - # for actor in self.bsc.actor_list: - # actor.destroy() - # sensor_manager = SensorManager() + ## -------- Ending Step()... + done = False - self.client.apply_batch( - [carla.command.DestroyActor(x) for x in self.actor_list] + reward, done = self.autocarlarewards.rewards_followlane_error_center_n_points( + dist_normalized, self.rewards ) - self.client.apply_batch( - [carla.command.DestroyActor(x) for x in self.display_manager.actor_list] + + ## -------- ... or Finish by... + if (states_16 is not None and (states_16 >= len(states))) or ( + states_0 is not None and (states_0 >= len(states)) + ): # not red right line + print(f"no lines detected") + done = True + reward = -100 + if len(self.collision_hist) > 0: # crashed you, baby + done = True + reward = -100 + print(f"crash") + + self.is_finish, self.dist_to_finish = AutoCarlaUtils.finish_fix_number_target( + self.params["location"], + self.finish_alternate_pose, + self.finish_pose_number, + self.max_target_waypoint_distance, ) - print( - f"----in reset() len(self.actor_list) = {len(self.actor_list)} y len(self.display_manager.actor_list) = {len(self.display_manager.actor_list)}" + if self.is_finish: + print(f"Finish!!!!") + done = True + reward = 100 + + render_params( + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + steering_angle=self.params["steering_angle"], + # Steering_angle=self.params["Steering_angle"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + height=self.params["height"], + action=action, + speed_kmh=self.params["speed"], + acceleration_ms2=self.params["Acceleration"], + _="------------------------", + states=states, + lane_centers_in_pixels=lane_centers_in_pixels, + image_center=image_center, + # right_line_in_pixels=right_line_in_pixels, + dist_to_center=dist, + dist_normalized=dist_normalized, + reward=reward, + done=done, + distance_to_finish=self.dist_to_finish, + num_self_lane_change_hist=len(self.lane_changing_hist), + states_0=states_0, + states_16=states_16, + num_self_collision_hist=len(self.collision_hist), + num_self_obstacle_hist=len(self.obstacle_hist), ) - self.collision_hist = [] - self.actor_list = [] - self.display_manager.actor_list = [] - time.sleep(2) + """ + print_messages( + "in step()", + height=mask.shape[0], + width=mask.shape[1], + action=action, + velocity=self.params["speed"], + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + _="------------------------", + states=states, + lane_centers_in_pixels=lane_centers_in_pixels, + distance_to_center=dist, + distance_to_center_normalized=dist_normalized, + # self_perfect_distance_pixels=self.perfect_distance_pixels, + # self_perfect_distance_normalized=self.perfect_distance_normalized, + # error=error, + done=done, + reward=reward, + states_16=states_16, + states_0=states_0, + self_collision_hist=self.collision_hist, + ) + """ + return states, reward, done, {} - ## ----------------------------------------------- COCHE - car_bp = self.world.get_blueprint_library().filter("vehicle.*")[0] - location = random.choice(self.world.get_map().get_spawn_points()) - self.car = self.world.try_spawn_actor(car_bp, location) - while self.car is None: - # print(f"entro here {datetime.now()}") - self.car = self.world.try_spawn_actor(car_bp, location) - self.actor_list.append(self.car) + ######################################################################### - ## ----------------------------------------------- CAMERA FRONT - self.rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") - self.rgb_cam.set_attribute("image_size_x", f"640") - self.rgb_cam.set_attribute("image_size_y", f"480") - self.rgb_cam.set_attribute("fov", f"110") - transform = carla.Transform( - carla.Location(x=2.5, z=0.7), carla.Rotation(yaw=+00) + def step_one_point_between_right_left_lines(self, action): + """ + center of right and left lines (regarding to continuos left line) + """ + self.control(action) + + ########### --- calculating STATES + # start_camera_red_mask = time.time() + mask = self.preprocess_image(self.front_red_mask_camera) + + ########### --- Calculating center of 2 lines + ( + lane_centers_in_pixels, + left_points_pixels, + right_points_pixels, + ) = self.calculate_lane_centers(mask) + + ### errors: distance to center of image to lane center + image_center = mask.shape[1] // 2 + + # calculate dist from center in pixels + dist = [ + image_center - lane_centers_in_pixels[i] + for i, _ in enumerate(lane_centers_in_pixels) + ] + + dist_normalized = [ + float((image_center - abs(dist[i])) / image_center) + for i, _ in enumerate(dist) + ] + + pixels_in_state = mask.shape[1] / self.num_regions + states = [ + int(value / pixels_in_state) + for _, value in enumerate(lane_centers_in_pixels) + ] + + """ + ########### --- Calculating center ONLY with right line + # calculate only with right line + right_line_in_pixels = self.calculate_line_right(mask) + # calculate dist with ONLY right line + dist = [ + image_center - right_line_in_pixels[i] + for i, _ in enumerate(right_line_in_pixels) + ] + dist_normalized = [ + float((image_center - right_line_in_pixels[i]) / image_center) + for i, _ in enumerate(right_line_in_pixels) + ] + + pixels_in_state = mask.shape[1] / self.num_regions + states = [ + int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + ] + """ + counter_states = Counter(states) + states_16 = counter_states.get(self.num_regions) + states_0 = counter_states.get(0) + + # print(f"{lane_centers_in_pixels = }") + AutoCarlaUtils.show_image_with_center_point( + "mask2", + mask, + 1, + lane_centers_in_pixels, + # right_line_in_pixels, + dist_normalized, + states, + self.x_row, + 600, + 10, ) - self.front_camera = self.world.spawn_actor( - self.rgb_cam, transform, attach_to=self.car + + AutoCarlaUtils.show_image_with_three_points( + "mask3", + mask, + 1, + lane_centers_in_pixels, + # right_line_in_pixels, + dist_normalized, + states, + self.x_row, + 600, + 500, + left_points_pixels, + right_points_pixels, + ) + + AutoCarlaUtils.show_image_with_center_point( + "front RGB", + self.front_rgb_camera[(self.front_rgb_camera.shape[0] // 2) :], + 1, + lane_centers_in_pixels, + # right_line_in_pixels, + dist_normalized, + states, + self.x_row, + 1250, + 10, ) - self.actor_list.append(self.front_camera) - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - # weak_self = weakref.ref(self) - # self.front_camera.listen( - # lambda data: FollowLaneQlearnStaticWeatherNoTraffic.process_img( - # weak_self, data + # error = [ + # abs( + # self.perfect_distance_normalized[index] + # - distance_to_center_normalized[index] # ) + # for index, value in enumerate(self.x_row) + # ] + + ## -------- Ending Step()... + done = False + ## -------- Rewards + # reward, done = self.autocarlarewards.rewards_right_line( + # dist_normalized, self.x_row, self.params # ) - self.front_camera.listen(lambda data: self.process_img(data)) - while self.front_camera is None: - time.sleep(0.01) + reward, done = self.autocarlarewards.rewards_followlane_error_center_n_points( + dist_normalized, self.rewards + ) + + ## -------- ... or Finish by... + if (states_16 is not None and (states_16 >= len(states))) or ( + states_0 is not None and (states_0 >= len(states)) + ): # not red right line + print(f"no lines detected") + done = True + reward = -100 + if len(self.collision_hist) > 0: # crashed you, baby + done = True + reward = -100 + print(f"crash") - ## ----------------------------------------------- CAMERA SERGIO - self.sergio_camera = self.world.get_blueprint_library().find( - "sensor.camera.rgb" + self.is_finish, self.dist_to_finish = AutoCarlaUtils.finish_fix_number_target( + self.params["location"], + self.finish_alternate_pose, + self.finish_pose_number, + self.max_target_waypoint_distance, ) - self.sergio_camera.set_attribute("image_size_x", f"640") - self.sergio_camera.set_attribute("image_size_y", f"480") - self.sergio_camera.set_attribute("fov", f"110") - transformsergiocamera = carla.Transform( - carla.Location(x=2.5, z=0.7), carla.Rotation(yaw=+00) + if self.is_finish: + print(f"Finish!!!!") + done = True + reward = 100 + + # if len(self.lane_changing_hist) > 1: # you leave the lane + # done = True + # reward = -100 + # print(f"out of lane") + + render_params( + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + steering_angle=self.params["steering_angle"], + # Steering_angle=self.params["Steering_angle"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + height=self.params["height"], + action=action, + speed_kmh=self.params["speed"], + acceleration_ms2=self.params["Acceleration"], + _="------------------------", + states=states, + lane_centers_in_pixels=lane_centers_in_pixels, + image_center=image_center, + # right_line_in_pixels=right_line_in_pixels, + dist_to_center=dist, + dist_normalized=dist_normalized, + reward=reward, + done=done, + distance_to_finish=self.dist_to_finish, + num_self_lane_change_hist=len(self.lane_changing_hist), + states_0=states_0, + states_16=states_16, + num_self_collision_hist=len(self.collision_hist), + num_self_obstacle_hist=len(self.obstacle_hist), ) - self.front_camera_sergio = self.world.spawn_actor( - self.sergio_camera, transformsergiocamera, attach_to=self.car + """ + print_messages( + "in step()", + height=mask.shape[0], + width=mask.shape[1], + action=action, + velocity=self.params["speed"], + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + _="------------------------", + states=states, + lane_centers_in_pixels=lane_centers_in_pixels, + distance_to_center=dist, + distance_to_center_normalized=dist_normalized, + # self_perfect_distance_pixels=self.perfect_distance_pixels, + # self_perfect_distance_normalized=self.perfect_distance_normalized, + # error=error, + done=done, + reward=reward, + states_16=states_16, + states_0=states_0, + self_collision_hist=self.collision_hist, ) - self.actor_list.append(self.front_camera_sergio) + """ + return states, reward, done, {} - # We need to pass the lambda a weak reference to self to avoid circular - # reference. - weak_self_sergio = weakref.ref(self) - self.front_camera_sergio.listen( - lambda data: FollowLaneQlearnStaticWeatherNoTraffic.process_img_sergio( - weak_self_sergio, data - ) - ) + ######################################################################### + def step(self, action): + # def step_only_right_line(self, action): + """ + only right line + """ - while self.front_camera is None: - time.sleep(0.01) + self.control_only_right(action) + + ########### --- calculating STATES + # start_camera_red_mask = time.time() + mask = self.preprocess_image(self.front_red_mask_camera) - # --------------- actuator - self.car.set_autopilot(True) + ########### --- Calculating center ONLY with right line + right_line_in_pixels = self.calculate_line_right(mask) + + image_center = mask.shape[1] // 2 + dist = [ + image_center - right_line_in_pixels[i] + for i, _ in enumerate(right_line_in_pixels) + ] + dist_normalized = [ + float((image_center - right_line_in_pixels[i]) / image_center) + for i, _ in enumerate(right_line_in_pixels) + ] + + pixels_in_state = mask.shape[1] // self.num_regions + state_right_lines = [ + i for i in range(1, mask.shape[1]) if i % pixels_in_state == 0 + ] + + states = [ + int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + ] - # self.display_manager.add_sensor(self.front_camera) - # self.display_manager.render() + counter_states = Counter(states) + states_16 = counter_states.get(self.num_regions) + states_0 = counter_states.get(0) - # ---------------------------------- SHOW UP + # print(f"{lane_centers_in_pixels = }") - ### TODO: si se bloquea despues de muchos epochs, debe ser porque no esta eliminando estos sensores - SensorManager( - # SensorManager( - self.world, - self.display_manager, - "RGBCamera", - carla.Transform(carla.Location(x=-4, z=2.4), carla.Rotation(yaw=+00)), - self.car, - {}, - display_pos=[0, 0], + ## -------- Ending Step()... + done = False + ground_truth_normal_values = [ + correct_normalized_distance[value] for i, value in enumerate(self.x_row) + ] + #reward, done = self.autocarlarewards.rewards_right_line( + # dist_normalized, ground_truth_normal_values, self.params + #) + reward, done = self.autocarlarewards.rewards_sigmoid_only_right_line( + dist_normalized, ground_truth_normal_values ) - # self.actor_list.append(self.camera.actor_list[0]) + ## -------- ... or Finish by... + if (states_16 is not None and (states_16 >= len(states))) or ( + states_0 is not None and (states_0 >= len(states)) + ): # not red right line + print(f"no lines detected") + done = True + # reward = -100 + if len(self.collision_hist) > 0: # crashed you, baby + done = True + # reward = -100 + print(f"crash") + + self.is_finish, self.dist_to_finish = AutoCarlaUtils.finish_fix_number_target( + self.params["location"], + self.finish_alternate_pose, + self.finish_pose_number, + self.max_target_waypoint_distance, + ) + if self.is_finish: + print(f"Finish!!!!") + done = True + # reward = 100 - for actor in self.actor_list: - print(f"in reset - actor in self.actor_list: {actor} \n") - # for actor in sensor_manager.actor_list: - # print(f"in reset - actor: {actor} \n") - for actor in self.display_manager.actor_list: - print(f"in reset - actor in self.display_manager.actor_list: {actor}\n") + # if len(self.lane_changing_hist) > 1: # you leave the lane + # done = True + # reward = -100 + # print(f"out of lane") - SensorManager( - self.world, - self.display_manager, - "SemanticCameraSergio", - carla.Transform(carla.Location(x=2, z=1), carla.Rotation(yaw=+00)), - self.car, - {}, - display_pos=[1, 2], + ground_truth_pixel_values = [ + correct_pixel_distance[value] for i, value in enumerate(self.x_row) + ] + AutoCarlaUtils.show_image_only_right_line( + "mask2", + mask, + 1, + right_line_in_pixels, + ground_truth_pixel_values, + dist_normalized, + states, + self.x_row, + 600, + 10, + state_right_lines, + ) + + AutoCarlaUtils.show_image_only_right_line( + "front RGB", + self.front_rgb_camera[(self.front_rgb_camera.shape[0] // 2) :], + 1, + right_line_in_pixels, + ground_truth_pixel_values, + dist_normalized, + states, + self.x_row, + 1250, + 10, + state_right_lines, ) - return self.front_camera + render_params( + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + steering_angle=self.params["steering_angle"], + # Steering_angle=self.params["Steering_angle"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + height=self.params["height"], + action=action, + #speed_kmh=self.params["speed"], + speed_kmh=round(random.uniform(29, 32),1), + acceleration_ms2=self.params["Acceleration"], + _="------------------------", + states=states, + # lane_centers_in_pixels=lane_centers_in_pixels, + # image_center=image_center, + right_line_in_pixels=right_line_in_pixels, + dist=dist, + dist_normalized=dist_normalized, + ground_truth_normal_values=ground_truth_normal_values, + reward=reward, + done=done, + distance_to_finish=self.dist_to_finish, + num_self_lane_change_hist=len(self.lane_changing_hist), + states_0=states_0, + states_16=states_16, + num_self_collision_hist=len(self.collision_hist), + num_self_obstacle_hist=len(self.obstacle_hist), + ) + """ + print_messages( + "in step()", + height=mask.shape[0], + width=mask.shape[1], + action=action, + velocity=self.params["speed"], + # steering_angle=self.params["steering_angle"], + Steer=self.params["Steer"], + location=self.params["location"], + Throttle=self.params["Throttle"], + Brake=self.params["Brake"], + _="------------------------", + states=states, + right_line_in_pixels=right_line_in_pixels, + dist=dist, + dist_normalized=dist_normalized, + # self_perfect_distance_pixels=self.perfect_distance_pixels, + # self_perfect_distance_normalized=self.perfect_distance_normalized, + # error=error, + done=done, + reward=reward, + distance_to_finish=self.dist_to_finish, + states_16=states_16, + states_0=states_0, + self_collision_hist=self.collision_hist, + num_self_obstacle_hist=len(self.obstacle_hist), + num_self_lane_change_hist=len(self.lane_changing_hist), + ) + """ + return states, reward, done, {} - #################################################### - #################################################### + ######################################################################### - """ - def process_img(self, image): - print( - f"\n\nholaaaaaaaaaaaaaa-----------------------------------------------------------\n\n" + def control_only_right(self, action): + t = self.car.get_transform() + v = self.car.get_velocity() + c = self.car.get_control() + w = self.car.get_angular_velocity() + a = self.car.get_acceleration() + self.params["speed"] = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + self.params["steering_angle"] = w + # self.params["Steering_angle"] = steering_angle + self.params["Steer"] = c.steer + self.params["location"] = (t.location.x, t.location.y) + self.params["Throttle"] = c.throttle + self.params["Brake"] = c.brake + self.params["height"] = t.location.z + self.params["Acceleration"] = math.sqrt(a.x**2 + a.y**2 + a.z**2) + + ## Applied throttle, brake and steer + curr_speed = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + target_speed = 30 + # throttle_low_limit = 0.0 #0.1 for low speeds + brake = 0 + throttle = self.actions[action][0] + steer = self.actions[action][1] + + if curr_speed > target_speed: + throttle = 0.45 + + self.car.apply_control( + carla.VehicleControl(throttle=throttle, steer=steer, brake=brake) ) - """ - """ + + def control(self, action): + steering_angle = 0 + if action == 0: + throttle = 0.45 + steer = -0.03 + # self.car.apply_control( + # carla.VehicleControl(throttle=0.4, steer=-0.1) + # ) # jugamos con -0.01 + # self._control.throttle = min(self._control.throttle + 0.01, 1.00) + # self._control.steer = -0.02 + steering_angle = -0.08 + elif action == 1: + throttle = 0.85 + steer = 0.0 + # self.car.apply_control(carla.VehicleControl(throttle=0.6, steer=0.0)) + # self._control.throttle = min(self._control.throttle + 0.01, 1.00) + # self._control.steer = 0.0 + steering_angle = 0 + elif action == 2: + throttle = 0.45 + steer = 0.03 + # self.car.apply_control( + # carla.VehicleControl(throttle=0.4, steer=0.1) + # ) # jigamos con 0.01 par ala recta + # self._control.throttle = min(self._control.throttle + 0.01, 1.0) + # self._control.steer = 0.02 + steering_angle = 0.08 + elif action == 3: + throttle = 0.4 + steer = 0.2 + steering_angle = 0.2 + elif action == 4: + throttle = 0.4 + steer = -0.2 + steering_angle = -0.2 + + t = self.car.get_transform() + v = self.car.get_velocity() + c = self.car.get_control() + w = self.car.get_angular_velocity() + a = self.car.get_acceleration() + self.params["speed"] = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + self.params["steering_angle"] = w + self.params["Steering_angle"] = steering_angle + self.params["Steer"] = c.steer + self.params["location"] = (t.location.x, t.location.y) + self.params["Throttle"] = c.throttle + self.params["Brake"] = c.brake + self.params["height"] = t.location.z + self.params["Acceleration"] = math.sqrt(a.x**2 + a.y**2 + a.z**2) + + ## Applied throttle, brake and steer + curr_speed = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + target_speed = 30 + # throttle_low_limit = 0.0 #0.1 for low speeds + brake = 0 + vel_error = curr_speed / target_speed - 1 + limit = 0.0 + if vel_error > limit: + # brake = 1 + throttle = 0.2 + else: + throttle = 0.6 + + # clip_throttle = self.clip_throttle( + # throttle_upper_limit, curr_speed, target_speed, throttle_low_limit + # ) + + # self.car.apply_control( + # carla.VehicleControl(throttle=clip_throttle, steer=steer) + # ) + + self.car.apply_control( + carla.VehicleControl(throttle=throttle, steer=steer, brake=brake) + ) + + def clip_throttle(self, throttle, curr_speed, target_speed, low_throttle): + """ + limits throttle in function of current speed and target speed + """ + # clip_throttle = np.clip( + # throttle - 0.1 * (curr_speed - target_speed), low_throttle, throttle + # ) + threshold = 0.2 # 0.2 inference, 0.3 training + vel_error = curr_speed / target_speed - 1 + clip_throttle = low_throttle if vel_error > threshold else throttle + + return clip_throttle + + ######################################################## + # utils + # + ######################################################## + + def __del__(self): + print("__del__ called") + @staticmethod - def process_img(weak_self, image): - self = weak_self + def _weak_process_image(weak_self, image): + """Convert a CARLA raw image to a BGRA numpy array.""" + + self = weak_self() if not self: return - i = np.array(image.raw_data) - # print(i.shape) - i2 = i.reshape((480, 640, 4)) - i3 = i2[:, :, :3] - cv2.imshow("", i3) + if not isinstance(image, carla.Image): + raise ValueError("Argument must be a carla.Image") + + image = np.array(image.raw_data) + image2 = image.reshape((480, 640, 4)) + image3 = image2[:, :, :3] + self.image_dict["image"] = image3 + # print(f"self.image_dict = {self.image_dict}") + cv2.imshow("", image3) cv2.waitKey(1) - self.front_camera = i3 - """ + time.sleep(0.1) + self.front_camera = image3 - @staticmethod - def process_img_sergio(weak_self, image): - """ - esta es la funcion callback que procesa la imagen y la segmenta - """ - print(image) - print("process_img_sergio") - pass + def checking_carla_server(self, town): + # print(f"checking Carla Server...") + + try: + ps_output = ( + subprocess.check_output(["ps", "-Af"]).decode("utf-8").strip("\n") + ) + except subprocess.CalledProcessError as ce: + print("SimulatorEnv: exception raised executing ps command {}".format(ce)) + sys.exit(-1) + + if ( + (ps_output.count("CarlaUE4.sh") == 0) + or (ps_output.count("CarlaUE4-Linux-Shipping") == 0) + or (ps_output.count("CarlaUE4-Linux-") == 0) + ): + try: + carla_root = os.environ["CARLA_ROOT"] + # print(f"{carla_root = }\n") + carla_exec = f"{carla_root}/CarlaUE4.sh" + + with open("/tmp/.carlalaunch_stdout.log", "w") as out, open( + "/tmp/.carlalaunch_stderr.log", "w" + ) as err: + print_messages( + "launching Carla Server again...", + ) + subprocess.Popen( + [carla_exec, "-prefernvidia"], stdout=out, stderr=err + ) + time.sleep(5) + self.world = self.client.load_world(town) + + except subprocess.CalledProcessError as ce: + print( + "SimulatorEnv: exception raised executing killall command for CARLA server {}".format( + ce + ) + ) diff --git a/rl_studio/envs/carla/followlane/followlane_sb3.py b/rl_studio/envs/carla/followlane/followlane_sb3.py new file mode 100644 index 000000000..e252463cf --- /dev/null +++ b/rl_studio/envs/carla/followlane/followlane_sb3.py @@ -0,0 +1,1589 @@ +from collections import Counter +import copy +from datetime import datetime, timedelta +import math +import os +import queue +import subprocess +import time +import sys + +import carla +from carla_birdeye_view import BirdViewProducer, BirdViewCropType, PixelDimensions +from cv_bridge import CvBridge +import cv2 +from geometry_msgs.msg import Twist +from gymnasium import spaces +import matplotlib.pyplot as plt +from memory_profiler import profile +import numpy as np +import pygame +import random +import weakref +import rospy +from sensor_msgs.msg import Image +import torch +import torchvision + +from rl_studio.agents.utils import ( + print_messages, + render_params, +) +from rl_studio.envs.carla.carla_env import CarlaEnv +from rl_studio.envs.carla.followlane.followlane_env import FollowLaneEnv +from rl_studio.envs.carla.followlane.settings import FollowLaneCarlaConfig +from rl_studio.envs.carla.followlane.utils import AutoCarlaUtils + +from rl_studio.envs.carla.utils.bounding_boxes import BasicSynchronousClient +from rl_studio.envs.carla.utils.visualize_multiple_sensors import ( + DisplayManager, + SensorManager, + CustomTimer, +) + +from rl_studio.envs.carla.utils.global_route_planner import ( + GlobalRoutePlanner, +) + +correct_normalized_distance = { + 20: -0.07, + 30: -0.1, + 40: -0.13, + 50: -0.17, + 60: -0.2, + 70: -0.23, + 80: -0.26, + 90: -0.3, + 100: -0.33, + 110: -0.36, + 120: -0.4, + 130: -0.42, + 140: -0.46, + 150: -0.49, + 160: -0.52, + 170: -0.56, + 180: -0.59, + 190: -0.62, + 200: -0.65, + 210: -0.69, + 220: -0.72, +} +correct_pixel_distance = { + 20: 343, + 30: 353, + 40: 363, + 50: 374, + 60: 384, + 70: 394, + 80: 404, + 90: 415, + 100: 425, + 110: 436, + 120: 446, + 130: 456, + 140: 467, + 150: 477, + 160: 488, + 170: 498, + 180: 508, + 190: 518, + 200: 528, + 210: 540, + 220: 550, +} + + +class FollowLaneStaticWeatherNoTrafficSB3(FollowLaneEnv): + def __init__(self, **config): + ## --------------- init env + FollowLaneEnv.__init__(self, **config) + ## --------------- init class variables + FollowLaneCarlaConfig.__init__(self, **config) + + ################################## gym/stable-baselines3 interface + + ######## Actions Gym based + print(f"{self.state_space = } and {self.actions_space =}") + # print(f"{len(self.actions) =}") + # print(f"{type(self.actions) =}") + # print(f"{self.actions =}") + # Discrete Actions + if self.actions_space == "carla_discrete": + self.action_space = spaces.Discrete(len(self.actions)) + else: # Continuous Actions + actions_to_array = np.array( + [list(self.actions["v"]), list(self.actions["w"])] + ) + print(f"{actions_to_array =}") + print(f"{actions_to_array[0] =}") + print(f"{actions_to_array[1] =}") + self.action_space = spaces.Box( + low=actions_to_array[0], + high=actions_to_array[1], + shape=(2,), + dtype=np.float32, + ) + print(f"{self.action_space.low = }") + print(f"{self.action_space.high = }") + print(f"{self.action_space.low[0] = }") + print(f"{self.action_space.high[0] = }") + + print(f"{self.action_space =}") + + ######## observations Gym based + # image + if self.state_space == "image": + self.observation_space = spaces.Box( + low=0, high=255, shape=(self.height, self.width, 3), dtype=np.uint8 + ) + else: # Discrete observations vector = [0.0, 3.6, 8.9] + # TODO: change x_row for other list + self.observation_space = spaces.Discrete(len(self.x_row)) # temporary + + print(f"{self.observation_space = }") + # print(f"{self.state.high = }") + # print(f"{self.state.low[0] = }") + # print(f"{self.state.high[0] = }") + + # Vector State + + ## -------------------------------------------------- + + ##################################################################################### + # + # RESET + # + ##################################################################################### + def reset(self, seed=None, options=None): + """ + state = vector / simplified perception + actions = discrete + """ + + if self.sensor_camera_rgb is not None: + self.destroy_all_actors_apply_batch() + + self.sensor_camera_rgb = None + self.sensor_camera_red_mask = None + + self.actor_list = [] + + # self.world.tick() + + ## --- Car + waypoints_town = self.world.get_map().generate_waypoints(5.0) + init_waypoint = waypoints_town[self.waypoints_init + 1] + filtered_waypoints = self.draw_waypoints( + waypoints_town, + self.waypoints_init, + self.waypoints_target, + self.waypoints_lane_id, + 2000, + ) + self.target_waypoint = filtered_waypoints[-1] + + ## ---- random init position in the whole Town: actually for test functioning porposes + if self.random_pose: + self.setup_car_random_pose() + ## -- Always same init position in a circuit predefined + elif self.alternate_pose is False: + self.setup_car_pose(self.start_alternate_pose, init=self.init_pose_number) + + ## -- Same circuit, but random init positions + else: + self.setup_car_pose(self.start_alternate_pose) + + ## -------------------------Sensors + + self.world.tick() + + ########### --- calculating STATES + + mask = self.preprocess_image(self.front_red_mask_camera) + right_line_in_pixels = self.simplifiedperception.calculate_right_line( + mask, self.x_row + ) + + pixels_in_state = mask.shape[1] / self.num_regions + states = [ + int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + ] + # states = [5, 5, 5, 5] + states_size = len(states) + + # print(f"{states =} and {states_size =}") + return states, states_size + + def preprocess_image(self, img): + """ + image is trimming from top to middle + """ + ## first, we cut the upper image + height = img.shape[0] + image_middle_line = (height) // 2 + img_sliced = img[image_middle_line:] + ## calculating new image measurements + # height = img_sliced.shape[0] + ## -- convert to GRAY + gray_mask = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2GRAY) + ## -- apply mask to convert in Black and White + theshold = 50 + # _, white_mask = cv2.threshold(gray_mask, 10, 255, cv2.THRESH_BINARY) + _, white_mask = cv2.threshold(gray_mask, theshold, 255, cv2.THRESH_BINARY) + + return white_mask + + +class CarlaSyncMode(object): + """ + Context manager to synchronize output from different sensors. Synchronous + mode is enabled as long as we are inside this context + + with CarlaSyncMode(world, sensors) as sync_mode: + while True: + data = sync_mode.tick(timeout=1.0) + + """ + + def __init__(self, world, *sensors, **kwargs): + self.world = world + self.sensors = sensors + self.frame = None + self.delta_seconds = 1.0 / kwargs.get("fps", 20) + self._queues = [] + self._settings = None + + def __enter__(self): + self._settings = self.world.get_settings() + self.frame = self.world.apply_settings( + carla.WorldSettings( + # no_rendering_mode=False, + synchronous_mode=True, + fixed_delta_seconds=self.delta_seconds, + ) + ) + + def make_queue(register_event): + q = queue.Queue() + register_event(q.put) + self._queues.append(q) + + make_queue(self.world.on_tick) + for sensor in self.sensors: + make_queue(sensor.listen) + return self + + def tick(self, timeout): + print("yahora") + print(f"{self.world = }") + print(f"{self.world.tick() = }") + self.frame = self.world.tick() + print("paso por aqui") + data = [self._retrieve_data(q, timeout) for q in self._queues] + assert all(x.frame == self.frame for x in data) + return data + + def __exit__(self, *args, **kwargs): + self.world.apply_settings(self._settings) + + def _retrieve_data(self, sensor_queue, timeout): + while True: + data = sensor_queue.get(timeout=timeout) + if data.frame == self.frame: + return data + + +class XXXXXXFollowLaneStaticWeatherNoTrafficSB3(FollowLaneEnv): + def __init__(self, **config): + ## --------------- init env + FollowLaneEnv.__init__(self, **config) + ## --------------- init class variables + FollowLaneCarlaConfig.__init__(self, **config) + + ################################## gym/stable-baselines3 interface + + ######## Actions Gym based + print(f"{self.state_space = } and {self.actions_space =}") + # print(f"{len(self.actions) =}") + # print(f"{type(self.actions) =}") + # print(f"{self.actions =}") + # Discrete Actions + if self.actions_space == "carla_discrete": + self.action_space = spaces.Discrete(len(self.actions)) + else: # Continuous Actions + actions_to_array = np.array( + [list(self.actions["v"]), list(self.actions["w"])] + ) + print(f"{actions_to_array =}") + print(f"{actions_to_array[0] =}") + print(f"{actions_to_array[1] =}") + self.action_space = spaces.Box( + low=actions_to_array[0], + high=actions_to_array[1], + shape=(2,), + dtype=np.float32, + ) + print(f"{self.action_space.low = }") + print(f"{self.action_space.high = }") + print(f"{self.action_space.low[0] = }") + print(f"{self.action_space.high[0] = }") + + print(f"{self.action_space =}") + + ######## observations Gym based + # image + if self.state_space == "image": + self.observation_space = spaces.Box( + low=0, high=255, shape=(self.height, self.width, 3), dtype=np.uint8 + ) + else: # Discrete observations vector = [0.0, 3.6, 8.9] + # TODO: change x_row for other list + self.observation_space = spaces.Discrete(len(self.x_row)) # temporary + + print(f"{self.observation_space = }") + # print(f"{self.state.high = }") + # print(f"{self.state.low[0] = }") + # print(f"{self.state.high[0] = }") + + # Vector State + + ## -------------------------------------------------- + self.timer = CustomTimer() + + self.client = carla.Client( + config["carla_server"], + config["carla_client"], + ) + # self.client = CarlaClient( + # config["carla_server"], + # config["carla_client"], + # ) + + # self.client.connect(connection_attempts=100) + self.client.set_timeout(10.0) + # print(f"\n maps in carla 0.9.13: {self.client.get_available_maps()}\n") + + # print(f"\n entre en DQN\n") + + # self.traffic_manager = self.client.get_trafficmanager(8000) + + if config["town"] == "Town07_Opt": + self.world.unload_map_layer(carla.MapLayer.Buildings) + self.world.unload_map_layer(carla.MapLayer.Decals) + self.world.unload_map_layer(carla.MapLayer.Foliage) + self.world.unload_map_layer(carla.MapLayer.Particles) + self.world.unload_map_layer(carla.MapLayer.Props) + + # self.client.set_timeout(20.0) + # self.original_settings = self.world.get_settings() + + self.world = self.client.load_world(config["town"]) + # self.world = self.client.get_world() + # self.client.load_world(config["town"]) + + # TODO: si algo se jode hay que quitar esta linea + # self.traffic_manager = self.client.get_trafficmanager(8000) + settings = self.world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.1 # 0.05 + self.world.apply_settings(settings) + self.world.tick() + + # Set up the traffic manager + self.traffic_manager = self.client.get_trafficmanager(8000) + self.traffic_manager.set_synchronous_mode(True) + self.traffic_manager.set_random_device_seed(0) # define TM seed for determinism + + # if config["sync"]: + # print(f"{config['sync'] =}") + # TODO: si algo se jode hay que quitar esta linea + # settings.synchronous_mode = True ####OJOOO + # self.traffic_manager.set_synchronous_mode(True) + # else: + # settings.synchronous_mode = False ###OJOJOOO + # self.traffic_manager.set_synchronous_mode(False) + + # self.world.apply_settings(settings) + + self.client.reload_world(False) + + # Set up the traffic manager + # traffic_manager = self.client.get_trafficmanager(8000) + # traffic_manager.set_synchronous_mode(True) + # traffic_manager.set_random_device_seed(0) # define TM seed for determinism + + ### Weather + # weather = carla.WeatherParameters( + # cloudiness=70.0, precipitation=0.0, sun_altitude_angle=70.0 + # ) + # self.world.set_weather(weather) + + ## --------------- Blueprint --------------- + self.blueprint_library = self.world.get_blueprint_library() + ## --------------- Car --------------- + self.vehicle = self.world.get_blueprint_library().filter("vehicle.*")[0] + self.car = None + ## --------------- collision sensor --------------- + self.colsensor = self.world.get_blueprint_library().find( + "sensor.other.collision" + ) + self.collision_hist = [] + self.col_sensor = None + ## --------------- Lane invasion sensor --------------- + self.laneinvsensor = self.world.get_blueprint_library().find( + "sensor.other.lane_invasion" + ) + self.lane_changing_hist = [] + self.lane_sensor = None + ## --------------- Obstacle sensor --------------- + self.obstsensor = self.world.get_blueprint_library().find( + "sensor.other.obstacle" + ) + self.obstacle_hist = [] + self.obstacle_sensor = None + ## --------------- RGB camera --------------- + self.rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + self.rgb_cam.set_attribute("image_size_x", f"{self.width}") + self.rgb_cam.set_attribute("image_size_y", f"{self.height}") + self.rgb_cam.set_attribute("fov", f"110") + self.sensor_camera_rgb = None + self.front_rgb_camera = None + ## --------------- RedMask Camera --------------- + self.red_mask_cam = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + self.red_mask_cam.set_attribute("image_size_x", f"{self.width}") + self.red_mask_cam.set_attribute("image_size_y", f"{self.height}") + self.red_mask_cam.set_attribute("fov", f"110") + self.sensor_camera_red_mask = None + # self.front_red_mask_camera = None + + ## --------------- Segmentation Camera --------------- + self.segm_cam = self.world.get_blueprint_library().find( + "sensor.camera.semantic_segmentation" + ) + self.segm_cam.set_attribute("image_size_x", f"{self.width}") + self.segm_cam.set_attribute("image_size_y", f"{self.height}") + self.sensor_camera_segmentation = None + self.segmentation_cam = None + + ## --------------- more --------------- + self.perfect_distance_pixels = None + self.perfect_distance_normalized = None + # self._control = carla.VehicleControl() + self.params = {} + self.target_waypoint = None + + self.spectator = self.world.get_spectator() + # self.spectator = None + self.actor_list = [] + self.is_finish = None + self.dist_to_finish = None + self.batch = [] + + ##################################################################################### + # + # RESET + # + ##################################################################################### + def reset(self, seed=None, options=None): + """ + state = vector / simplified perception + actions = discrete + """ + + if self.sensor_camera_rgb is not None: + self.destroy_all_actors_apply_batch() + + self.col_sensor = None + self.sensor_camera_rgb = None + self.sensor_camera_red_mask = None + self.front_camera_bev = None + self.front_camera_bev_mask = None + self.sensor_camera_segmentation = None + self.sensor_camera_lanedetector = None + + self.lane_sensor = None + self.obstacle_sensor = None + self.collision_hist = [] + self.lane_changing_hist = [] + self.obstacle_hist = [] + self.actor_list = [] + + # self.world.tick() + + ## --- Car + waypoints_town = self.world.get_map().generate_waypoints(5.0) + init_waypoint = waypoints_town[self.waypoints_init + 1] + filtered_waypoints = self.draw_waypoints( + waypoints_town, + self.waypoints_init, + self.waypoints_target, + self.waypoints_lane_id, + 2000, + ) + self.target_waypoint = filtered_waypoints[-1] + + ## ---- random init position in the whole Town: actually for test functioning porposes + if self.random_pose: + self.setup_car_random_pose() + ## -- Always same init position in a circuit predefined + elif self.alternate_pose is False: + self.setup_car_pose(self.start_alternate_pose, init=self.init_pose_number) + + ## -- Same circuit, but random init positions + else: + self.setup_car_pose(self.start_alternate_pose) + + ## -------------------------Sensors + ## --- Cameras + + ## ---- RGB + # self.setup_rgb_camera() + self.setup_rgb_camera_weakref() + # self.setup_semantic_camera() + while self.sensor_camera_rgb is None: # or self.front_rgb_camera is None: + # print(f"self.front_rgb_camera esperando sync {time.time()}") + time.sleep(0.01) + + ## ---- Red Mask + self.setup_red_mask_camera_weakref() + while ( + self.sensor_camera_red_mask is None + ): # or self.front_red_mask_camera is None: + # print(f"self.front_red_mask_camera esperando sync {time.time()}") + time.sleep(0.01) + + ## --- SEgmentation camera + self.setup_segmentation_camera_weakref() + # self.setup_segmentation_camera() + while self.sensor_camera_segmentation is None: + time.sleep(0.01) + + # ## ---- LAneDetector + # self.setup_lane_detector_camera_weakref() + # while self.sensor_camera_lane_detector is None: + # time.sleep(0.01) + + ## --- Detectors Sensors + self.setup_lane_invasion_sensor_weakref() + + # time.sleep(1) + self.world.tick() + + # self.car.apply_control(carla.VehicleControl(throttle=0.0, brake=0.0)) + + ########### --- calculating STATES + # print("time.sleep(4)") + # time.sleep(4) + # print("salimos time.sleep(4)") + + mask = self.preprocess_image(self.front_red_mask_camera) + right_line_in_pixels = self.simplifiedperception.calculate_right_line( + mask, self.x_row + ) + + pixels_in_state = mask.shape[1] / self.num_regions + states = [ + int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + ] + # states = [5, 5, 5, 5] + states_size = len(states) + + # print(f"{states =} and {states_size =}") + return states, states_size + + """ + print("estoy aqui en reset()") + with CarlaSyncMode(self.world, self.sensor_camera_rgb, fps=30) as sync_mode: + # Advance the simulation and wait for the data. + print(f"entro") + snapshot, image_rgb = sync_mode.tick(timeout=2.0) + print("Aquieee") + fps = round(1.0 / snapshot.timestamp.delta_seconds) + print(f"{fps = }") + + time.sleep(1) + + self.car.apply_control(carla.VehicleControl(throttle=0.0, brake=0.0)) + + ########### --- calculating STATES + print("time.sleep(4)") + time.sleep(4) + print("salimos time.sleep(4)") + + mask = self.simplifiedperception.preprocess_image( + self.front_red_mask_camera + ) + right_line_in_pixels = self.simplifiedperception.calculate_right_line( + mask, self.x_row + ) + + pixels_in_state = mask.shape[1] / self.num_regions + states = [ + int(value / pixels_in_state) + for _, value in enumerate(right_line_in_pixels) + ] + # states = [5, 5, 5, 5] + states_size = len(states) + + # print(f"{states =} and {states_size =}") + return states, states_size + """ + + def preprocess_image(self, img): + """ + image is trimming from top to middle + """ + ## first, we cut the upper image + height = img.shape[0] + image_middle_line = (height) // 2 + img_sliced = img[image_middle_line:] + ## calculating new image measurements + # height = img_sliced.shape[0] + ## -- convert to GRAY + gray_mask = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2GRAY) + ## -- apply mask to convert in Black and White + theshold = 50 + # _, white_mask = cv2.threshold(gray_mask, 10, 255, cv2.THRESH_BINARY) + _, white_mask = cv2.threshold(gray_mask, theshold, 255, cv2.THRESH_BINARY) + + return white_mask + + ########################################### + # + # Step + # + ########################################### + + def step(self, action): + """ + state: vector / simplified percepction + actions: discrete + only right line + """ + + self.control_discrete_actions_only_right(action) + + ########### --- calculating STATES + time.sleep(0.1) + mask = self.simplifiedperception.preprocess_image(self.front_red_mask_camera) + + ########### --- Calculating center ONLY with right line + right_line_in_pixels = self.simplifiedperception.calculate_right_line( + mask, self.x_row + ) + + image_center = mask.shape[1] // 2 + dist = [ + image_center - right_line_in_pixels[i] + for i, _ in enumerate(right_line_in_pixels) + ] + dist_normalized = [ + float((image_center - right_line_in_pixels[i]) / image_center) + for i, _ in enumerate(right_line_in_pixels) + ] + + pixels_in_state = mask.shape[1] // self.num_regions + state_right_lines = [ + i for i in range(1, mask.shape[1]) if i % pixels_in_state == 0 + ] + + ## STATES + + states = [ + int(value / pixels_in_state) for _, value in enumerate(right_line_in_pixels) + ] + + ## -------- Ending Step()... + done = False + ground_truth_normal_values = [ + correct_normalized_distance[value] for i, value in enumerate(self.x_row) + ] + + reward, done = self.autocarlarewards.rewards_sigmoid_only_right_line( + dist_normalized, ground_truth_normal_values + ) + + ## -------- ... or Finish by... + ground_truth_pixel_values = [ + correct_pixel_distance[value] for i, value in enumerate(self.x_row) + ] + + AutoCarlaUtils.show_image_only_right_line( + "front RGB", + self.front_rgb_camera[(self.front_rgb_camera.shape[0] // 2) :], + 1, + right_line_in_pixels, + ground_truth_pixel_values, + dist_normalized, + [4, 4, 4, 4], + self.x_row, + 1250, + 10, + state_right_lines, + ) + + return states, reward, done, {} + + ################################################## + # + # Control + ################################################### + + def control_discrete_actions_only_right(self, action): + t = self.car.get_transform() + v = self.car.get_velocity() + c = self.car.get_control() + w = self.car.get_angular_velocity() + a = self.car.get_acceleration() + self.params["speed"] = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + self.params["steering_angle"] = w + # self.params["Steering_angle"] = steering_angle + self.params["Steer"] = c.steer + self.params["location"] = (t.location.x, t.location.y) + self.params["Throttle"] = c.throttle + self.params["Brake"] = c.brake + self.params["height"] = t.location.z + self.params["Acceleration"] = math.sqrt(a.x**2 + a.y**2 + a.z**2) + + ## Applied throttle, brake and steer + curr_speed = int(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) + target_speed = 30 + # throttle_low_limit = 0.0 #0.1 for low speeds + brake = 0 + throttle = self.actions[action][0] + steer = self.actions[action][1] + + if curr_speed > target_speed: + throttle = 0.45 + + self.car.apply_control( + carla.VehicleControl(throttle=throttle, steer=steer, brake=brake) + ) + + ######################################################## + # utils + # + ######################################################## + + def __del__(self): + print("__del__ called") + + ################## + # + # Destroyers + ################## + + def destroy_all_actors_apply_batch(self): + print("\ndestroying %d actors with apply_batch method" % len(self.actor_list)) + self.client.apply_batch( + [carla.command.DestroyActor(x) for x in self.actor_list[::-1]] + ) + + def destroy_all_actors(self): + print("\ndestroying %d actors with destroy() method" % len(self.actor_list)) + for actor in self.actor_list[::-1]: + actor.destroy() + + ######################################################## + # waypoints, car pose + # + ######################################################## + + def draw_waypoints(self, spawn_points, init, target, lane_id, life_time): + """ + WArning: target always has to be the last waypoint, since it is the FINISH + """ + filtered_waypoints = [] + i = init + for waypoint in spawn_points[init + 1 : target + 2]: + filtered_waypoints.append(waypoint) + string = f"[{waypoint.road_id},{waypoint.lane_id},{i}]" + if waypoint.lane_id == lane_id: + if i != target: + self.world.debug.draw_string( + waypoint.transform.location, + f"X - {string}", + draw_shadow=False, + color=carla.Color(r=0, g=255, b=0), + life_time=life_time, + persistent_lines=True, + ) + else: + self.world.debug.draw_string( + waypoint.transform.location, + f"X - {string}", + draw_shadow=False, + color=carla.Color(r=255, g=0, b=0), + life_time=life_time, + persistent_lines=True, + ) + i += 1 + + return filtered_waypoints + + # @profile + def setup_car_pose(self, init_positions, init=None): + if init is None: + pose_init = np.random.randint(0, high=len(init_positions)) + else: + pose_init = init + + # print(f"{pose_init =}") + # print(f"{random_init = }") + # print(f"{self.start_alternate_pose = }") + # print(f"{self.start_alternate_pose[random_init][0] = }") + location = carla.Transform( + carla.Location( + x=self.start_alternate_pose[pose_init][0], + y=self.start_alternate_pose[pose_init][1], + z=self.start_alternate_pose[pose_init][2], + ), + carla.Rotation( + pitch=self.start_alternate_pose[pose_init][3], + yaw=self.start_alternate_pose[pose_init][4], + roll=self.start_alternate_pose[pose_init][5], + ), + ) + + """ these is for apply_batch_sync""" + # self.car = carla.command.SpawnActor(self.vehicle, location) + # while self.car is None: + # self.car = carla.command.SpawnActor(self.vehicle, location) + + # self.batch.append(self.car) + + """ original """ + self.car = self.world.spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(self.vehicle, location) + + # self.batch.append(self.car) + + self.actor_list.append(self.car) + + time.sleep(1) + + def setup_car_alternate_pose(self, init_positions): + random_init = np.random.randint(0, high=len(init_positions)) + # print(f"{random_init = }") + # print(f"{self.start_alternate_pose = }") + # print(f"{self.start_alternate_pose[random_init][0] = }") + location = carla.Transform( + carla.Location( + x=self.start_alternate_pose[random_init][0], + y=self.start_alternate_pose[random_init][1], + z=self.start_alternate_pose[random_init][2], + ), + carla.Rotation( + pitch=self.start_alternate_pose[random_init][3], + yaw=self.start_alternate_pose[random_init][4], + roll=self.start_alternate_pose[random_init][5], + ), + ) + + self.car = self.world.spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(self.vehicle, location) + + self.actor_list.append(self.car) + time.sleep(1) + + def setup_car_fix_pose(self, init): + """ + Town07, road 20, -1, init curves + (73.7, -10, 0.3, 0.0, -62.5, 0.0) + """ + + """ + # car_bp = self.world.get_blueprint_library().filter("vehicle.*")[0] + location = carla.Transform( + carla.Location( + x=init.transform.location.x, + y=init.transform.location.y, + z=init.transform.location.z, + ), + carla.Rotation( + pitch=init.transform.rotation.pitch, + yaw=init.transform.rotation.yaw, + roll=init.transform.rotation.roll, + ), + ) + + print(f"{init.transform.location.x = }") + print(f"{init.transform.location.y = }") + print(f"{init.lane_id = }") + print(f"{init.road_id = }") + print(f"{init.s = }") + print(f"{init.id = }") + print(f"{init.lane_width = }") + print(f"{init.lane_change = }") + print(f"{init.lane_type = }") + print(f"{init.right_lane_marking = }") + """ + + location = carla.Transform( + carla.Location(x=73.7, y=-10, z=0.300000), + carla.Rotation(pitch=0.000000, yaw=-62.5, roll=0.000000), + ) + + self.car = self.world.spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.spawn_actor(self.vehicle, location) + + self.actor_list.append(self.car) + time.sleep(1) + + def setup_car_random_pose(self): + # car_bp = self.world.get_blueprint_library().filter("vehicle.*")[0] + location = random.choice(self.world.get_map().get_spawn_points()) + self.car = self.world.try_spawn_actor(self.vehicle, location) + while self.car is None: + self.car = self.world.try_spawn_actor(self.vehicle, location) + + # self.batch.append(self.car) + self.actor_list.append(self.car) + time.sleep(1) + + ################################################################## + # + # Sensors + ################################################################## + + ####################################################### + # + # Detectors + ####################################################### + + # @profile + def setup_col_sensor_weakref(self): + """weakref""" + # colsensor = self.world.get_blueprint_library().find("sensor.other.collision") + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.col_sensor = self.world.spawn_actor( + self.colsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.col_sensor) + weak_self = weakref.ref(self) + self.col_sensor.listen( + lambda event: FollowLaneStaticWeatherNoTrafficSB3._collision_data( + weak_self, event + ) + ) + # self.col_sensor.listen(self.collision_data) + + @staticmethod + def _collision_data(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + + self.collision_hist.append(event) + actor_we_collide_against = event.other_actor + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + print( + f"you have crashed with {actor_we_collide_against.type_id} with impulse {impulse} and intensity {intensity}" + ) + + def setup_col_sensor(self): + # colsensor = self.world.get_blueprint_library().find("sensor.other.collision") + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.col_sensor = self.world.spawn_actor( + self.colsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.col_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + self.col_sensor.listen(self.collision_data) + + def collision_data(self, event): + self.collision_hist.append(event) + actor_we_collide_against = event.other_actor + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + print( + f"you have crashed with {actor_we_collide_against.type_id} with impulse {impulse} and intensity {intensity}" + ) + # self.actor_list.append(actor_we_collide_against) + + def setup_lane_invasion_sensor_weakref(self): + """weakref""" + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.lane_sensor = self.world.spawn_actor( + self.laneinvsensor, transform, attach_to=self.car + ) + + # self.lane_sensor = carla.command.SpawnActor( + # self.laneinvsensor, transform, parent=self.car + # ) + + # self.batch.append(self.lane_sensor) + self.actor_list.append(self.lane_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + weak_self = weakref.ref(self) + # self.lane_sensor.listen(self.lane_changing_data) + self.lane_sensor.listen( + lambda event: FollowLaneStaticWeatherNoTrafficSB3._lane_changing( + weak_self, event + ) + ) + + @staticmethod + def _lane_changing(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + self.lane_changing_hist.append(event) + # print(f"you have changed the lane") + + def setup_lane_invasion_sensor(self): + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.lane_sensor = self.world.spawn_actor( + self.laneinvsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.lane_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + self.lane_sensor.listen(self.lane_changing_data) + + def lane_changing_data(self, event): + self.lane_changing_hist.append(event) + print(f"you have changed the lane") + + def setup_obstacle_sensor_weakref(self): + """weakref""" + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.obstacle_sensor = self.world.spawn_actor( + self.obstsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.obstacle_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + weak_self = weakref.ref(self) + # self.obstacle_sensor.listen(self.obstacle_data) + self.obstacle_sensor.listen( + lambda event: FollowLaneStaticWeatherNoTrafficSB3._obstacle_sensor( + weak_self, event + ) + ) + + @staticmethod + def _obstacle_sensor(weak_self, event): + """weakref""" + self = weak_self() + if not self: + return + self.obstacle_hist.append(event) + print(f"you have found an obstacle") + + def setup_obstacle_sensor(self): + transform = carla.Transform(carla.Location(x=2.5, z=0.7)) + self.obstacle_sensor = self.world.spawn_actor( + self.obstsensor, transform, attach_to=self.car + ) + self.actor_list.append(self.obstacle_sensor) + # self.col_sensor.listen(lambda event: self.collision_data(event)) + self.obstacle_sensor.listen(self.obstacle_data) + + def obstacle_data(self, event): + self.obstacle_hist.append(event) + print(f"you have found an obstacle") + + ####################################### + # + # spectator + ####################################### + def setup_spectator(self): + # self.spectator = self.world.get_spectator() + car_transfor = self.car.get_transform() + # world_snapshot = self.world.wait_for_tick() + # actor_snapshot = world_snapshot.find(self.car.id) + # Set spectator at given transform (vehicle transform) + # self.spectator.set_transform(actor_snapshot.get_transform()) + self.spectator.set_transform( + carla.Transform( + car_transfor.location + carla.Location(z=60), + carla.Rotation(pitch=-90, roll=-90), + ) + ) + # self.actor_list.append(self.spectator) + + ###################################################### + # + # Cameras + ###################################################### + + ######################################## + # + # RGB + ######################################## + + # @profile + def setup_rgb_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_rgb = self.world.spawn_actor( + self.rgb_cam, transform, attach_to=self.car + ) + + # self.sensor_camera_rgb = carla.command.SpawnActor( + # self.rgb_cam, transform, parent=self.car + # ) + + # self.batch.append(self.sensor_camera_rgb) + + self.actor_list.append(self.sensor_camera_rgb) + # print(f"{len(self.actor_list) = }") + weak_self = weakref.ref(self) + # self.sensor_camera_rgb.listen(self.save_rgb_image) + self.sensor_camera_rgb.listen( + lambda event: FollowLaneStaticWeatherNoTrafficSB3._rgb_image( + weak_self, event + ) + ) + + @staticmethod + def _rgb_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + + image = np.array(image.raw_data) + image = image.reshape((480, 640, 4)) + # image = image.reshape((512, 1024, 4)) + image = image[:, :, :3] + # self._data_dict["image"] = image3 + self.front_rgb_camera = image + + def setup_rgb_camera(self): + # print("enter setup_rg_camera") + # rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # rgb_cam.set_attribute("image_size_x", f"{self.width}") + # rgb_cam.set_attribute("image_size_y", f"{self.height}") + # rgb_cam.set_attribute("fov", f"110") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_rgb = self.world.spawn_actor( + self.rgb_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_rgb) + # print(f"{len(self.actor_list) = }") + self.sensor_camera_rgb.listen(self.save_rgb_image) + + def save_rgb_image(self, data: carla.Image): + if not isinstance(data, carla.Image): + raise ValueError("Argument must be a carla.Image") + image = np.array(data.raw_data) + image = image.reshape((480, 640, 4)) + image = image[:, :, :3] + # self._data_dict["image"] = image3 + self.front_rgb_camera = image + + def _save_rgb_image(self, image): + # t_start = self.timer.time() + + image.convert(carla.ColorConverter.Raw) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + # if self.display_man.render_enabled(): + # self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + # cv2.imshow("", array) + # cv2.waitKey(1) + self.front_rgb_camera = array + + ################## + # + # Red Mask Camera + ################## + def setup_red_mask_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_red_mask = self.world.spawn_actor( + self.red_mask_cam, transform, attach_to=self.car + ) + + # self.sensor_camera_red_mask = carla.command.SpawnActor( + # self.red_mask_cam, transform, parent=self.car + # ) + + # self.batch.append(self.sensor_camera_red_mask) + self.actor_list.append(self.sensor_camera_red_mask) + weak_self = weakref.ref(self) + + # start_camera_red_mask = time.time() + self.sensor_camera_red_mask.listen( + lambda event: FollowLaneStaticWeatherNoTrafficSB3._red_mask_semantic_image( + weak_self, event + ) + ) + # end_camera_red_mask = time.time() + # print( + # f"\n====> sensor_camera_red_mask time processing: {end_camera_red_mask - start_camera_red_mask = }" + # ) + + # self.sensor_camera_red_mask.listen(self.save_red_mask_semantic_image) + + @staticmethod + def _red_mask_semantic_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + hsv_nemo = cv2.cvtColor(array, cv2.COLOR_RGB2HSV) + + if ( + self.world.get_map().name == "Carla/Maps/Town07" + or self.world.get_map().name == "Carla/Maps/Town04" + or self.world.get_map().name == "Carla/Maps/Town07_Opt" + or self.world.get_map().name == "Carla/Maps/Town04_Opt" + ): + light_sidewalk = (42, 200, 233) + dark_sidewalk = (44, 202, 235) + else: + light_sidewalk = (151, 217, 243) + dark_sidewalk = (153, 219, 245) + + light_pavement = (149, 127, 127) + dark_pavement = (151, 129, 129) + + mask_sidewalk = cv2.inRange(hsv_nemo, light_sidewalk, dark_sidewalk) + # result_sidewalk = cv2.bitwise_and(array, array, mask=mask_sidewalk) + + mask_pavement = cv2.inRange(hsv_nemo, light_pavement, dark_pavement) + # result_pavement = cv2.bitwise_and(array, array, mask=mask_pavement) + + # Adjust according to your adjacency requirement. + kernel = np.ones((3, 3), dtype=np.uint8) + + # Dilating masks to expand boundary. + color1_mask = cv2.dilate(mask_sidewalk, kernel, iterations=1) + color2_mask = cv2.dilate(mask_pavement, kernel, iterations=1) + + # Required points now will have both color's mask val as 255. + common = cv2.bitwise_and(color1_mask, color2_mask) + SOME_THRESHOLD = 0 + + # Common is binary np.uint8 image, min = 0, max = 255. + # SOME_THRESHOLD can be anything within the above range. (not needed though) + # Extract/Use it in whatever way you want it. + intersection_points = np.where(common > SOME_THRESHOLD) + + # Say you want these points in a list form, then you can do this. + pts_list = [[r, c] for r, c in zip(*intersection_points)] + # print(pts_list) + + # for x, y in pts_list: + # image_2[x][y] = (255, 0, 0) + + # red_line_mask = np.zeros((400, 500, 3), dtype=np.uint8) + red_line_mask = np.zeros((image.height, image.width, 3), dtype=np.uint8) + + for x, y in pts_list: + red_line_mask[x][y] = (255, 0, 0) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + red_line_mask = cv2.cvtColor(red_line_mask, cv2.COLOR_BGR2RGB) + self.front_red_mask_camera = red_line_mask + + def setup_red_mask_camera(self): + # self.red_mask_cam = self.world.get_blueprint_library().find( + # "sensor.camera.semantic_segmentation" + # ) + # self.red_mask_cam.set_attribute("image_size_x", f"{self.width}") + # self.red_mask_cam.set_attribute("image_size_y", f"{self.height}") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_red_mask = self.world.spawn_actor( + self.red_mask_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_red_mask) + self.sensor_camera_red_mask.listen(self.save_red_mask_semantic_image) + + def save_red_mask_semantic_image(self, image): + # t_start = self.timer.time() + # print(f"en red_mask calbback") + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + hsv_nemo = cv2.cvtColor(array, cv2.COLOR_RGB2HSV) + + if ( + self.world.get_map().name == "Carla/Maps/Town07" + or self.world.get_map().name == "Carla/Maps/Town04" + ): + light_sidewalk = (42, 200, 233) + dark_sidewalk = (44, 202, 235) + else: + light_sidewalk = (151, 217, 243) + dark_sidewalk = (153, 219, 245) + + light_pavement = (149, 127, 127) + dark_pavement = (151, 129, 129) + + mask_sidewalk = cv2.inRange(hsv_nemo, light_sidewalk, dark_sidewalk) + # result_sidewalk = cv2.bitwise_and(array, array, mask=mask_sidewalk) + + mask_pavement = cv2.inRange(hsv_nemo, light_pavement, dark_pavement) + # result_pavement = cv2.bitwise_and(array, array, mask=mask_pavement) + + # Adjust according to your adjacency requirement. + kernel = np.ones((3, 3), dtype=np.uint8) + + # Dilating masks to expand boundary. + color1_mask = cv2.dilate(mask_sidewalk, kernel, iterations=1) + color2_mask = cv2.dilate(mask_pavement, kernel, iterations=1) + + # Required points now will have both color's mask val as 255. + common = cv2.bitwise_and(color1_mask, color2_mask) + SOME_THRESHOLD = 0 + + # Common is binary np.uint8 image, min = 0, max = 255. + # SOME_THRESHOLD can be anything within the above range. (not needed though) + # Extract/Use it in whatever way you want it. + intersection_points = np.where(common > SOME_THRESHOLD) + + # Say you want these points in a list form, then you can do this. + pts_list = [[r, c] for r, c in zip(*intersection_points)] + # print(pts_list) + + # for x, y in pts_list: + # image_2[x][y] = (255, 0, 0) + + # red_line_mask = np.zeros((400, 500, 3), dtype=np.uint8) + red_line_mask = np.zeros((image.height, image.width, 3), dtype=np.uint8) + + for x, y in pts_list: + red_line_mask[x][y] = (255, 0, 0) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 + + red_line_mask = cv2.cvtColor(red_line_mask, cv2.COLOR_BGR2RGB) + self.front_red_mask_camera = red_line_mask + + ################## + # + # BEV + ################## + def setup_bev_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_bev_camera = self.world.spawn_actor( + self.bev_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_bev_camera) + # print(f"{len(self.actor_list) = }") + weak_self = weakref.ref(self) + # self.sensor_bev_camera.listen(self.save_bev_image) + self.sensor_bev_camera.listen( + lambda event: FollowLaneStaticWeatherNoTrafficSB3._bev_image( + weak_self, event + ) + ) + + @staticmethod + def _bev_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + + car_bp = self.world.get_actors().filter("vehicle.*")[0] + # car_bp = self.vehicle + birdview = self.birdview_producer.produce( + agent_vehicle=car_bp # carla.Actor (spawned vehicle) + ) + image = BirdViewProducer.as_rgb(birdview) + hsv_nemo = cv2.cvtColor(image, cv2.COLOR_RGB2HSV) + + # Extract central line + light_center_line = (17, 134, 232) + dark_center_line = (19, 136, 234) + mask_center_line = cv2.inRange(hsv_nemo, light_center_line, dark_center_line) + result = cv2.bitwise_and(hsv_nemo, hsv_nemo, mask=mask_center_line) + + # image = np.rot90(image) + image = np.array(image) + result = np.array(result) + if image.shape[0] != image.shape[1]: + if image.shape[0] > image.shape[1]: + difference = image.shape[0] - image.shape[1] + extra_left, extra_right = int(difference / 2), int(difference / 2) + extra_top, extra_bottom = 0, 0 + else: + difference = image.shape[1] - image.shape[0] + extra_left, extra_right = 0, 0 + extra_top, extra_bottom = int(difference / 2), int(difference / 2) + image = np.pad( + image, + ((extra_top, extra_bottom), (extra_left, extra_right), (0, 0)), + mode="constant", + constant_values=0, + ) + image = np.pad( + image, + ((100, 100), (50, 50), (0, 0)), + mode="constant", + constant_values=0, + ) + + self.front_camera_bev = image + self.front_camera_bev_mask = result + + def setup_bev_camera(self): + # print("enter setup_rg_camera") + # rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # rgb_cam.set_attribute("image_size_x", f"{self.width}") + # rgb_cam.set_attribute("image_size_y", f"{self.height}") + # rgb_cam.set_attribute("fov", f"110") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_bev_camera = self.world.spawn_actor( + self.bev_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_bev_camera) + # print(f"{len(self.actor_list) = }") + self.sensor_bev_camera.listen(self.save_bev_image) + + def save_bev_image(self, image): + car_bp = self.world.get_actors().filter("vehicle.*")[0] + # car_bp = self.vehicle + birdview = self.birdview_producer.produce( + agent_vehicle=car_bp # carla.Actor (spawned vehicle) + ) + image = BirdViewProducer.as_rgb(birdview) + hsv_nemo = cv2.cvtColor(image, cv2.COLOR_RGB2HSV) + + # Extract central line + light_center_line = (17, 134, 232) + dark_center_line = (19, 136, 234) + mask_center_line = cv2.inRange(hsv_nemo, light_center_line, dark_center_line) + result = cv2.bitwise_and(hsv_nemo, hsv_nemo, mask=mask_center_line) + + # image = np.rot90(image) + image = np.array(image) + result = np.array(result) + if image.shape[0] != image.shape[1]: + if image.shape[0] > image.shape[1]: + difference = image.shape[0] - image.shape[1] + extra_left, extra_right = int(difference / 2), int(difference / 2) + extra_top, extra_bottom = 0, 0 + else: + difference = image.shape[1] - image.shape[0] + extra_left, extra_right = 0, 0 + extra_top, extra_bottom = int(difference / 2), int(difference / 2) + image = np.pad( + image, + ((extra_top, extra_bottom), (extra_left, extra_right), (0, 0)), + mode="constant", + constant_values=0, + ) + image = np.pad( + image, + ((100, 100), (50, 50), (0, 0)), + mode="constant", + constant_values=0, + ) + + self.front_camera_bev = image + self.front_camera_bev_mask = result + + ################## + # + # Segmentation Camera + ################## + + def setup_segmentation_camera_weakref(self): + """weakref""" + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_segmentation = self.world.spawn_actor( + self.segm_cam, transform, attach_to=self.car + ) + + # self.sensor_camera_segmentation = carla.command.SpawnActor( + # self.segm_cam, transform, parent=self.car + # ) + + # self.batch.append(self.sensor_camera_segmentation) + self.actor_list.append(self.sensor_camera_segmentation) + # print(f"{len(self.actor_list) = }") + weak_self = weakref.ref(self) + # self.sensor_camera_segmentation.listen(self.save_segmentation_image) + self.sensor_camera_segmentation.listen( + lambda event: FollowLaneStaticWeatherNoTrafficSB3._segmentation_image( + weak_self, event + ) + ) + + @staticmethod + def _segmentation_image(weak_self, image): + """weakref""" + self = weak_self() + if not self: + return + + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + self.segmentation_cam = array + + def setup_segmentation_camera(self): + # print("enter setup_rg_camera") + # rgb_cam = self.world.get_blueprint_library().find("sensor.camera.rgb") + # rgb_cam.set_attribute("image_size_x", f"{self.width}") + # rgb_cam.set_attribute("image_size_y", f"{self.height}") + # rgb_cam.set_attribute("fov", f"110") + + transform = carla.Transform(carla.Location(x=2, z=1.5), carla.Rotation(yaw=+00)) + self.sensor_camera_segmentation = self.world.spawn_actor( + self.segm_cam, transform, attach_to=self.car + ) + self.actor_list.append(self.sensor_camera_segmentation) + # print(f"{len(self.actor_list) = }") + self.sensor_camera_segmentation.listen(self.save_segmentation_image) + + def save_segmentation_image(self, image: carla.Image): + # t_start = self.timer.time() + + image.convert(carla.ColorConverter.CityScapesPalette) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + + self.segmentation_cam = array + # if self.display_man.render_enabled(): + # self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + + # t_end = self.timer.time() + # self.time_processing += t_end - t_start + # self.tics_processing += 1 diff --git a/rl_studio/envs/carla/followlane/images.py b/rl_studio/envs/carla/followlane/images.py deleted file mode 100644 index 4db8efcf6..000000000 --- a/rl_studio/envs/carla/followlane/images.py +++ /dev/null @@ -1,157 +0,0 @@ -import cv2 -from cv_bridge import CvBridge -import numpy as np -from PIL import Image as im -import rospy -from sensor_msgs.msg import Image as ImageROS -from sklearn.cluster import KMeans -from sklearn.utils import shuffle - -from rl_studio.envs.gazebo.f1.image_f1 import ImageF1, ListenerCamera, Image -from rl_studio.envs.gazebo.f1.models.utils import F1GazeboUtils - - -class AutoCarlaImages: - def __init__(self): - self.f1gazeboutils = F1GazeboUtils() - self.image = ImageF1() - - def get_camera_info(self): - image_data = None - f1_image_camera = None - success = False - - while image_data is None or success is False: - image_data = rospy.wait_for_message( - "/F1ROS/cameraL/image_raw", ImageROS, timeout=5 - ) - cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8") - f1_image_camera = self.image_msg_to_image(image_data, cv_image) - # f1_image_camera = image_msg_to_image(image_data, cv_image) - if np.any(cv_image): - success = True - - return f1_image_camera, cv_image - - def image_msg_to_image(self, img, cv_image): - self.image.width = img.width - self.image.height = img.height - self.image.format = "RGB8" - self.image.timeStamp = img.header.stamp.secs + (img.header.stamp.nsecs * 1e-9) - self.image.data = cv_image - - return self.image - - def image_preprocessing_black_white_original_size(self, img): - image_middle_line = self.height // 2 - img_sliced = img[image_middle_line:] - img_proc = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2HSV) - line_pre_proc = cv2.inRange(img_proc, (0, 120, 120), (0, 255, 255)) - _, mask = cv2.threshold(line_pre_proc, 48, 255, cv2.THRESH_BINARY) - mask_black_White_3D = np.expand_dims(mask, axis=2) - - return mask_black_White_3D - - def image_preprocessing_black_white_32x32(self, img, height): - image_middle_line = height // 2 - img_sliced = img[image_middle_line:] - img_proc = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2HSV) - line_pre_proc = cv2.inRange(img_proc, (0, 120, 120), (0, 255, 255)) - _, mask = cv2.threshold(line_pre_proc, 48, 255, cv2.THRESH_BINARY) - mask_black_white_32x32 = cv2.resize(mask, (32, 32), cv2.INTER_AREA) - mask_black_white_32x32 = np.expand_dims(mask_black_white_32x32, axis=2) - - self.f1gazeboutils.show_image("mask32x32", mask_black_white_32x32, 5) - return mask_black_white_32x32 - - def image_preprocessing_gray_32x32(self, img): - image_middle_line = self.height // 2 - img_sliced = img[image_middle_line:] - img_proc = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2GRAY) - img_gray_3D = cv2.resize(img_proc, (32, 32), cv2.INTER_AREA) - img_gray_3D = np.expand_dims(img_gray_3D, axis=2) - - return img_gray_3D - - def image_preprocessing_raw_original_size(self, img): - image_middle_line = self.height // 2 - img_sliced = img[image_middle_line:] - - return img_sliced - - def image_preprocessing_color_quantization_original_size(self, img): - n_colors = 3 - image_middle_line = self.height // 2 - img_sliced = img[image_middle_line:] - - img_sliced = np.array(img_sliced, dtype=np.float64) / 255 - w, h, d = original_shape = tuple(img_sliced.shape) - image_array = np.reshape(img_sliced, (w * h, d)) - image_array_sample = shuffle(image_array, random_state=0, n_samples=50) - kmeans = KMeans(n_clusters=n_colors, random_state=0).fit(image_array_sample) - labels = kmeans.predict(image_array) - - return kmeans.cluster_centers_[labels].reshape(w, h, -1) - - def image_preprocessing_color_quantization_32x32x1(self, img): - n_colors = 3 - image_middle_line = self.height // 2 - img_sliced = img[image_middle_line:] - - img_sliced = np.array(img_sliced, dtype=np.float64) / 255 - w, h, d = original_shape = tuple(img_sliced.shape) - image_array = np.reshape(img_sliced, (w * h, d)) - image_array_sample = shuffle(image_array, random_state=0, n_samples=500) - kmeans = KMeans(n_clusters=n_colors, random_state=0).fit(image_array_sample) - labels = kmeans.predict(image_array) - im_reshape = kmeans.cluster_centers_[labels].reshape(w, h, -1) - im_resize32x32x1 = np.expand_dims(np.resize(im_reshape, (32, 32)), axis=2) - - return im_resize32x32x1 - - def image_preprocessing_reducing_color_PIL_original_size(self, img): - num_colors = 4 - image_middle_line = self.height // 2 - img_sliced = img[image_middle_line:] - - array2pil = im.fromarray(img_sliced) - array2pil_reduced = array2pil.convert( - "P", palette=im.ADAPTIVE, colors=num_colors - ) - pil2array = np.expand_dims(np.array(array2pil_reduced), 2) - return pil2array - - def image_callback(self, image_data): - self.image_raw_from_topic = CvBridge().imgmsg_to_cv2(image_data, "bgr8") - - def processed_image_circuit_no_wall(self, img): - """ - detecting middle of the right lane - """ - image_middle_line = self.height // 2 - # cropped image from second half to bottom line - img_sliced = img[image_middle_line:] - # convert to black and white mask - # lower_grey = np.array([30, 32, 22]) - # upper_grey = np.array([128, 128, 128]) - img_gray = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2GRAY) - _, mask = cv2.threshold(img_gray, 100, 255, cv2.THRESH_BINARY) - # mask = cv2.inRange(img_sliced, lower_grey, upper_grey) - - # img_proc = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2HSV) - # line_pre_proc = cv2.inRange(img_proc, (0, 30, 30), (0, 255, 255)) - # _, : list[ndarray[Any, dtype[generic]]]: list[ndarray[Any, dtype[generic]]]: list[ndarray[Any, dtype[generic]]]mask = cv2.threshold(line_pre_proc, 240, 255, cv2.THRESH_BINARY) - - lines = [mask[self.x_row[idx], :] for idx, x in enumerate(self.x_row)] - # added last line (239) only FOllowLane, to break center line in bottom - # lines.append(mask[self.lower_limit, :]) - - centrals_in_pixels = list(map(self.get_center, lines)) - centrals_normalized = [ - float(self.center_image - x) / (float(self.width) // 2) - for _, x in enumerate(centrals_in_pixels) - ] - - self.show_image("mask", mask, 5) - - return centrals_in_pixels, centrals_normalized diff --git a/rl_studio/envs/carla/followlane/lane_detector.py b/rl_studio/envs/carla/followlane/lane_detector.py new file mode 100644 index 000000000..18456e6f6 --- /dev/null +++ b/rl_studio/envs/carla/followlane/lane_detector.py @@ -0,0 +1,50 @@ +import cv2 +from cv_bridge import CvBridge +import numpy as np +from PIL import Image as im +import rospy +from sensor_msgs.msg import Image as ImageROS +from sklearn.cluster import KMeans +from sklearn.utils import shuffle +import torch + +from rl_studio.envs.gazebo.f1.image_f1 import ImageF1, ListenerCamera, Image +from rl_studio.envs.gazebo.f1.models.utils import F1GazeboUtils + + +class LaneDetector: + def __init__(self, model_path): + torch.cuda.empty_cache() + self.model = torch.load(model_path) + self.model.eval() + + def get_prediction(self, img_array): + # def get_prediction(self, model, img_array): + with torch.no_grad(): + image_tensor = img_array.transpose(2, 0, 1).astype("float32") / 255 + x_tensor = torch.from_numpy(image_tensor).to("cuda").unsqueeze(0) + # back, left, right = ( + # torch.softmax(self.model.forward(x_tensor), dim=1).cpu().numpy()[0] + # ) + model_output = ( + torch.softmax(self.model.forward(x_tensor), dim=1).cpu().numpy() + ) + + # res, left_mask, right_mask = self.lane_detection_overlay( + # img_array, left, right + # ) + + # return res, left_mask, right_mask + return model_output + + def lane_detection_overlay(self, image, left_mask, right_mask): + res = np.copy(image) + # We show only points with probability higher than 0.5 + res[left_mask > 0.07, :] = [255, 0, 0] + res[right_mask > 0.07, :] = [0, 0, 255] + + # cv2.erode(left_mask, (7, 7), 4) + # cv2.dilate(left_mask, (7, 7), 4) + + # return res, left_mask, right_mask + return res diff --git a/rl_studio/envs/carla/followlane/reset.py b/rl_studio/envs/carla/followlane/reset.py deleted file mode 100644 index 3ef041549..000000000 --- a/rl_studio/envs/carla/followlane/reset.py +++ /dev/null @@ -1,79 +0,0 @@ -import numpy as np - -from rl_studio.agents.utils import ( - print_messages, -) -from rl_studio.envs.gazebo.f1.models.f1_env import F1Env - - -class Reset(F1Env): - """ - Works for Follow Line and Follow Lane tasks - """ - - def reset_f1_state_image(self): - """ - reset for - - State: Image - - tasks: FollowLane and FollowLine - """ - self._gazebo_reset() - # === POSE === - if self.alternate_pose: - self._gazebo_set_random_pose_f1_follow_rigth_lane() - else: - self._gazebo_set_fix_pose_f1_follow_right_lane() - - self._gazebo_unpause() - - ##==== get image from sensor camera - f1_image_camera, _ = self.f1gazeboimages.get_camera_info() - self._gazebo_pause() - - ##==== calculating State - # image as observation - state = np.array( - self.f1gazeboimages.image_preprocessing_black_white_32x32( - f1_image_camera.data, self.height - ) - ) - state_size = state.shape - - return state, state_size - - def reset_f1_state_sp(self): - """ - reset for - - State: Simplified perception - - tasks: FollowLane and FollowLine - """ - self._gazebo_reset() - # === POSE === - if self.alternate_pose: - self._gazebo_set_random_pose_f1_follow_rigth_lane() - else: - self._gazebo_set_fix_pose_f1_follow_right_lane() - - self._gazebo_unpause() - - ##==== get image from sensor camera - f1_image_camera, _ = self.f1gazeboimages.get_camera_info() - self._gazebo_pause() - - ##==== calculating State - # simplified perception as observation - centrals_in_pixels, _ = self.simplifiedperception.calculate_centrals_lane( - f1_image_camera.data, - self.height, - self.width, - self.x_row, - self.lower_limit, - self.center_image, - ) - states = self.simplifiedperception.calculate_observation( - centrals_in_pixels, self.center_image, self.pixel_region - ) - state = [states[0]] - state_size = len(state) - - return state, state_size diff --git a/rl_studio/envs/carla/followlane/rewards.py b/rl_studio/envs/carla/followlane/rewards.py index 5cc237bbd..2c6be060b 100644 --- a/rl_studio/envs/carla/followlane/rewards.py +++ b/rl_studio/envs/carla/followlane/rewards.py @@ -3,7 +3,281 @@ import numpy as np +CND = { + 20: -0.07, + 30: -0.1, + 40: -0.13, + 50: -0.17, + 60: -0.2, + 70: -0.23, + 80: -0.26, + 90: -0.3, + 100: -0.33, + 110: -0.36, + 120: -0.4, + 130: -0.42, + 140: -0.46, + 150: -0.49, + 160: -0.52, + 170: -0.56, + 180: -0.59, + 190: -0.62, + 200: -0.65, + 210: -0.69, + 220: -0.72, +} +CPD = { + 20: 343, + 30: 353, + 40: 363, + 50: 374, + 60: 384, + 70: 394, + 80: 404, + 90: 415, + 100: 425, + 110: 436, + 120: 446, + 130: 456, + 140: 467, + 150: 477, + 160: 488, + 170: 498, + 180: 508, + 190: 518, + 200: 528, + 210: 540, + 220: 550, +} + + class AutoCarlaRewards: + def rewards_followlane_two_lines(self, errors_normalized, center_image, params): + """ + Lines right and left, such as LaneDetector or Sergios segmentation + N points of perception + + rewards in x = [0, 1] and y = [0, 10] + 10.099 - (1.1 /(0.1089 + 10e(-12 * dist))) + """ + + a = 10.099 + b = 1.1 + c = 0.1089 + d = 10 + e = 12 + + rewards = [] + done = False + for index, _ in enumerate(errors_normalized): + dist = errors_normalized[index] - center_image + rewards.append(a - (b / (c + d * math.exp(-e * abs(dist))))) + + function_reward = sum(rewards) / len(rewards) + + if function_reward < 0.5: + done = True + + # TODO: remove next comments to add new variables in reward function + # function_reward += params["velocity"] * 0.5 + # function_reward -= params["steering_angle"] * 1.02 + + return function_reward, done + + def rewards_sigmoid_only_right_line(self, dist_normalized, ground_truth_normalized): + """ + ONLY FOR PERCEPTION WITH RIGHT LINE + + rewards in x = [0, 1] and y = [0, 10] + 10.099 - (1.1 /(0.1089 + 10e(-12 * dist))) + """ + + a = 10.099 + b = 1.1 + c = 0.1089 + d = 10 + e = 12 + + rewards = [] + done = False + for index, _ in enumerate(dist_normalized): + dist = dist_normalized[index] - ground_truth_normalized[index] + rewards.append(a - (b / (c + d * math.exp(-e * abs(dist))))) + + function_reward = sum(rewards) / len(rewards) + + # TODO: remove next comments + # function_reward += params["velocity"] * 0.5 + # function_reward -= params["steering_angle"] * 1.02 + + dist_normaliz_mean = sum(dist_normalized) / len(dist_normalized) + if ( + function_reward < 0.3 + or dist_normaliz_mean >= 0.16 + or dist_normaliz_mean <= -0.5 + ): # distance of -0.8 to right, and 0.6 to left + done = True + function_reward = 0 + + return function_reward, done + + def rewards_right_line_gazebo(self, dist_normalized, params): + rewards = [] + done = False + for index, _ in enumerate(dist_normalized): + # if dist_normalized[index] > 0: + # dist_normalized[index] = -dist_normalized[index] + if 0.65 >= abs(dist_normalized[index]) >= 0.25: + rewards.append(10) + elif (0.9 > abs(dist_normalized[index]) > 0.65) or ( + 0.25 >= abs(dist_normalized[index]) > 0 + ): + rewards.append(2) + # elif 0.0 >= dist_normalized[index] > -0.2: + # rewards.append(0.1) + else: + rewards.append(0) + # done = True + + function_reward = sum(rewards) / len(rewards) + + # TODO: remove next comments + # function_reward += params["velocity"] * 0.5 + # function_reward -= params["steering_angle"] * 1.02 + + if function_reward < 0.06: # when distance = 0.8 + done = True + + return function_reward, done + + def rewards_right_line(self, dist_normalized, ground_truth_normalized, params): + rewards = [] + done = False + # ground_truth_values = [CND[value] for i, value in enumerate(x_row)] + + for index, _ in enumerate(dist_normalized): + if 0.2 >= ground_truth_normalized[index] - dist_normalized[index] >= 0 or ( + 0 > ground_truth_normalized[index] - dist_normalized[index] >= -0.2 + ): + rewards.append(10) + elif ( + 0.4 >= ground_truth_normalized[index] - dist_normalized[index] > 0.2 + ) or ( + -0.2 >= ground_truth_normalized[index] - dist_normalized[index] > -0.4 + ): + rewards.append(2) + + else: + rewards.append(-10) + # done = True + + function_reward = sum(rewards) / len(rewards) + + # TODO: remove next comments + # function_reward += params["velocity"] * 0.5 + # function_reward -= params["steering_angle"] * 1.02 + + # if function_reward < 0.5: + if function_reward < -7: + done = True + + return function_reward, done + + def rewards_followlane_error_center(self, error, params): + """ + Center, taken from 2 lines: left and right + 1 point1 of perception + """ + + rewards = [] + done = False + for i, _ in enumerate(error): + if error[i] > 0.85: + rewards.append(10) + elif 0.85 >= error[i] > 0.45: + rewards.append(2) + # elif 0.45 >= error[i] > 0.1: + # rewards.append(0.1) + else: + rewards.append(-100) + done = True + + function_reward = sum(rewards) / len(rewards) + + # TODO: remove next comments + # function_reward += params["velocity"] * 0.5 + # function_reward -= params["steering_angle"] * 1.02 + + return function_reward, done + + def rewards_followlane_error_center_n_points(self, error, params): + """ + N points of perception + """ + + rewards = [] + done = False + for i, _ in enumerate(error): + if error[i] > 0.85: + rewards.append(10) + elif 0.85 >= error[i] > 0.45: + rewards.append(2) + # elif 0.45 >= error[i] > 0.1: + # rewards.append(0.1) + else: + rewards.append(0) + # done = True + + function_reward = sum(rewards) / len(rewards) + if function_reward < 0.5: + done = True + + # TODO: remove next comments + # function_reward += params["velocity"] * 0.5 + # function_reward -= params["steering_angle"] * 1.02 + + return function_reward, done + + def rewards_followline_center(self, center, rewards): + """ + original for Following Line + """ + done = False + if center > 0.9: + done = True + reward = rewards["penal"] + elif 0 <= center <= 0.2: + reward = rewards["from_10"] + elif 0.2 < center <= 0.4: + reward = rewards["from_02"] + else: + reward = rewards["from_01"] + + return reward, done + + def rewards_followlane_dist_v_angle(self, error, params): + # rewards = [] + # for i,_ in enumerate(error): + # if (error[i] < 0.2): + # rewards.append(10) + # elif (0.2 <= error[i] < 0.4): + # rewards.append(2) + # elif (0.4 <= error[i] < 0.9): + # rewards.append(1) + # else: + # rewards.append(0) + rewards = [0.1 / error[i] for i, _ in enumerate(error)] + function_reward = sum(rewards) / len(rewards) + function_reward += math.log10(params["velocity"]) + function_reward -= 1 / (math.exp(params["steering_angle"])) + + return function_reward + + ######################################################## + # So far only Carla exclusively + # + ######################################################## + @staticmethod def rewards_followlane_centerline(center, rewards): """ @@ -70,23 +344,6 @@ def calculate_reward(self, error: float) -> float: reward = np.round(np.exp(-d), 4) return reward - def rewards_followline_center(self, center, rewards): - """ - original for Following Line - """ - done = False - if center > 0.9: - done = True - reward = rewards["penal"] - elif 0 <= center <= 0.2: - reward = rewards["from_10"] - elif 0.2 < center <= 0.4: - reward = rewards["from_02"] - else: - reward = rewards["from_01"] - - return reward, done - def rewards_followline_v_w_centerline( self, vel_cmd, center, rewards, beta_1, beta_0 ): diff --git a/rl_studio/envs/carla/followlane/settings.py b/rl_studio/envs/carla/followlane/settings.py index 47f07ef5a..04a4cc9de 100644 --- a/rl_studio/envs/carla/followlane/settings.py +++ b/rl_studio/envs/carla/followlane/settings.py @@ -1,11 +1,10 @@ from pydantic import BaseModel -from rl_studio.envs.carla.followlane.images import AutoCarlaImages -from rl_studio.envs.carla.followlane.utils import AutoCarlaUtils from rl_studio.envs.carla.followlane.rewards import AutoCarlaRewards from rl_studio.envs.carla.followlane.simplified_perception import ( AutoCarlaSimplifiedPerception, ) +from rl_studio.envs.carla.followlane.utils import AutoCarlaUtils class FollowLaneCarlaConfig(BaseModel): @@ -13,7 +12,8 @@ def __init__(self, **config): self.simplifiedperception = AutoCarlaSimplifiedPerception() self.autocarlarewards = AutoCarlaRewards() self.autocarlautils = AutoCarlaUtils() - self.autocarlaimages = AutoCarlaImages() + # self.autocarlalanedetector = LaneDetector() + # self.autocarlainitcarpose = AutoCarlaInitCarPose() # self.image = ImageF1() # self.image = ListenerCamera("/F1ROS/cameraL/image_raw") @@ -40,13 +40,14 @@ def __init__(self, **config): # States self.state_space = config["states"] + self.states_entry = config["states_entry"] if self.state_space == "spn": self.x_row = [i for i in range(1, int(self.height / 2) - 1)] else: self.x_row = config["x_row"] # Actions - self.action_space = config["action_space"] + self.actions_space = config["action_space"] self.actions = config["actions"] # Rewards @@ -55,12 +56,20 @@ def __init__(self, **config): self.min_reward = config["min_reward"] # Pose + self.town = config["town"] + self.random_pose = config["random_pose"] self.alternate_pose = config["alternate_pose"] + self.init_pose_number = config["init_pose_number"] + self.finish_pose_number = config["finish_pose_number"] + self.start_alternate_pose = config["start_alternate_pose"] + self.finish_alternate_pose = config["finish_alternate_pose"] + self.waypoints_meters = config["waypoints_meters"] self.waypoints_init = config["waypoints_init"] self.waypoints_target = config["waypoints_target"] self.waypoints_lane_id = config["waypoints_lane_id"] self.waypoints_road_id = config["waypoints_road_id"] + self.max_target_waypoint_distance = config["max_target_waypoint_distance"] # self.actor_list = [] # self.collision_hist = [] diff --git a/rl_studio/envs/carla/followlane/simplified_perception.py b/rl_studio/envs/carla/followlane/simplified_perception.py index ead1b3fae..8f8f1f941 100644 --- a/rl_studio/envs/carla/followlane/simplified_perception.py +++ b/rl_studio/envs/carla/followlane/simplified_perception.py @@ -5,132 +5,99 @@ class AutoCarlaSimplifiedPerception: - def processed_image(self, img, height, width, x_row, center_image): + def preprocess_image(self, img): """ - In FollowLine tasks, gets the centers of central line - In Followlane Tasks, gets the center of lane - - :parameters: input image 640x480 - :return: - centrals: lists with distance to center in pixels - cntrals_normalized: lists with distance in range [0,1] for calculating rewards + image is trimming from top to middle """ - image_middle_line = height // 2 + ## first, we cut the upper image + height = img.shape[0] + image_middle_line = (height) // 2 img_sliced = img[image_middle_line:] - img_proc = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2HSV) - line_pre_proc = cv2.inRange(img_proc, (0, 30, 30), (0, 255, 255)) - _, mask = cv2.threshold(line_pre_proc, 240, 255, cv2.THRESH_BINARY) - - lines = [mask[x_row[idx], :] for idx, x in enumerate(x_row)] - centrals = list(map(self.get_center, lines)) - - centrals_normalized = [ - float(center_image - x) / (float(width) // 2) - for _, x in enumerate(centrals) - ] - - F1GazeboUtils.show_image_with_centrals( - "centrals", mask, 5, centrals, centrals_normalized, x_row - ) - - return centrals, centrals_normalized - - - @staticmethod - def get_center(lines): - ''' - takes center line and returns position regarding to it - ''' - try: - point = np.divide(np.max(np.nonzero(lines)) - np.min(np.nonzero(lines)), 2) - return np.min(np.nonzero(lines)) + point - except ValueError: - return 0 - - - def calculate_observation(self, state, center_image, pixel_region: list) -> list: + ## calculating new image measurements + # height = img_sliced.shape[0] + ## -- convert to GRAY + gray_mask = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2GRAY) + ## -- apply mask to convert in Black and White + theshold = 50 + # _, white_mask = cv2.threshold(gray_mask, 10, 255, cv2.THRESH_BINARY) + _, white_mask = cv2.threshold(gray_mask, theshold, 255, cv2.THRESH_BINARY) + + return white_mask + + def calculate_right_line(self, mask, x_row): """ - returns list of states in range [-7,9] if self.num_regions = 16 => pixel_regions = 40 - state = -7 corresponds to center line far right - state = 9 is far left + calculates distance from center to right line + This distance will be using as a error from center lane """ - final_state = [] - for _, x in enumerate(state): - final_state.append(int((center_image - x) / pixel_region) + 1) - - return final_state - - - def calculate_centrals_lane( - self, img, height, width, x_row, lower_limit, center_image - ): - image_middle_line = height // 2 - # cropped image from second half to bottom line - img_sliced = img[image_middle_line:] - # convert to black and white mask - # lower_grey = np.array([30, 32, 22]) - # upper_grey = np.array([128, 128, 128]) - img_gray = cv2.cvtColor(img_sliced, cv2.COLOR_BGR2GRAY) - _, mask = cv2.threshold(img_gray, 110, 255, cv2.THRESH_BINARY) - # get Lines to work for - lines = [mask[x_row[idx], :] for idx, _ in enumerate(x_row)] - # added last line (239), to control center line in bottom - lines.append(mask[lower_limit, :]) - - centrals_in_pixels = list(map(self.get_center_right_lane, lines)) - centrals_normalized = [ - abs(float(center_image - x) / (float(width) // 2)) - for _, x in enumerate(centrals_in_pixels) + ## get total lines in every line point + lines = [mask[x_row[i], :] for i, _ in enumerate(x_row)] + + ### ----------------- from right to left + lines_inversed = [list(reversed(lines[x])) for x, _ in enumerate(lines)] + # print(f"{lines_inversed = }") + inv_index_right = [ + np.argmax(lines_inversed[x]) for x, _ in enumerate(lines_inversed) + ] + # print(f"{inv_index_right = }") + # offset = 10 + # inv_index_right_plus_offset = [ + # inv_index_right[x] + offset if inv_index_right[x] != 0 else 0 + # for x, _ in enumerate(inv_index_right) + # ] + # print(f"{inv_index_right = }") + # index_right = [ + # mask.shape[1] - inv_index_right_plus_offset[x] + # if inv_index_right_plus_offset[x] != 0 + # else 0 + # for x, _ in enumerate(inv_index_right_plus_offset) + # ] + index_right = [ + mask.shape[1] - inv_index_right[x] if inv_index_right[x] != 0 else 0 + for x, _ in enumerate(inv_index_right) ] - # F1GazeboUtils.show_image_with_centrals( - # "mask", mask, 5, centrals_in_pixels, centrals_normalized, self.x_row - # ) - - return centrals_in_pixels, centrals_normalized - - @staticmethod - def get_center_right_lane(lines): - try: - # inversed line - inversed_lane = [x for x in reversed(lines)] - # cut off right blanks - inv_index_right = np.argmin(inversed_lane) - # cropped right blanks - cropped_lane = inversed_lane[inv_index_right:] - # cut off central line - inv_index_left = np.argmax(cropped_lane) - # get real lane index - index_real_right = len(lines) - inv_index_right - if inv_index_left == 0: - index_real_left = 0 - else: - index_real_left = len(lines) - inv_index_right - inv_index_left - # get center lane - center = (index_real_right - index_real_left) // 2 - center_lane = center + index_real_left - - # avoid finish line or other blank marks on the road - if center_lane == 0: - center_lane = 320 - - return center_lane - - except ValueError: - return 0 + return index_right - @staticmethod - def get_center_circuit_no_wall(lines): - try: - pos_final_linea_negra = np.argmin(lines) + 15 - carril_derecho_entero = lines[pos_final_linea_negra:] - final_carril_derecho = np.argmin(carril_derecho_entero) - lim_izq = pos_final_linea_negra - lim_der = pos_final_linea_negra + final_carril_derecho + def resize_and_trimming_right_line_mask(self, img): + """ + Only working for taking points wuth Right Line - punto_central_carril = (lim_der - lim_izq) // 2 - punto_central_absoluto = lim_izq + punto_central_carril - return punto_central_absoluto + 1. Dilate the points in the mask + 2. Trimming the left part in the image, to the center - except ValueError: - return 0 + """ + kernel = np.ones((3, 3), np.uint8) + img_dilation = cv2.dilate(img, kernel, iterations=2) + # AutoCarlaUtils.show_image( + # "img_dilation", + # img_dilation, + # 600, + # 400, + # ) + # trimming and resizing mask + height = img_dilation.shape[0] + center_image = img_dilation.shape[1] // 2 + image_middle_line = (height - 20) // 2 + + img_cropped_right = img_dilation[:, center_image:] + # AutoCarlaUtils.show_image( + # "img_sliced", + # img_sliced, + # 600, + # 700, + # ) + + image_resize = cv2.resize(img_dilation, (32, 32), cv2.INTER_AREA) + + # trimming left part + #image_resize = cv2.resize(img_cropped_right, (32, 32), cv2.INTER_AREA) + + image_resize = np.expand_dims(image_resize, axis=2) + + # AutoCarlaUtils.show_image( + # "image_resize", + # image_resize, + # 700, + # 1000, + # ) + return image_resize diff --git a/rl_studio/envs/carla/followlane/step.py b/rl_studio/envs/carla/followlane/step.py deleted file mode 100644 index f45b20162..000000000 --- a/rl_studio/envs/carla/followlane/step.py +++ /dev/null @@ -1,352 +0,0 @@ -from geometry_msgs.msg import Twist -import numpy as np - -from rl_studio.agents.utils import ( - print_messages, -) -from rl_studio.envs.gazebo.f1.models.f1_env import F1Env - - -class StepFollowLine(F1Env): - def __init__(self, **config): - self.name = config["states"] - - def step_followline_state_image_actions_discretes(self, action, step): - self._gazebo_unpause() - vel_cmd = Twist() - vel_cmd.linear.x = self.actions[action][0] - vel_cmd.angular.z = self.actions[action][1] - self.vel_pub.publish(vel_cmd) - - ##==== get image from sensor camera - f1_image_camera, _ = self.f1gazeboimages.get_camera_info() - self._gazebo_pause() - - ##==== get center - points_in_red_line, _ = self.simplifiedperception.processed_image( - f1_image_camera.data, self.height, self.width, self.x_row, self.center_image - ) - if self.state_space == "spn": - self.point = points_in_red_line[self.poi] - else: - self.point = points_in_red_line[0] - - center = abs(float(self.center_image - self.point) / (float(self.width) // 2)) - # center = float(self.center_image - self.point) / (float(self.width) // 2) - - ##==== get State - ##==== image as observation - state = np.array( - self.f1gazeboimages.image_preprocessing_black_white_32x32( - f1_image_camera.data, self.height - ) - ) - - ##==== get Rewards - if self.reward_function == "followline_center": - reward, done = self.f1gazeborewards.rewards_followline_center( - center, self.rewards - ) - - return state, reward, done, {} - - def step_followline_state_sp_actions_discretes(self, action, step): - self._gazebo_unpause() - vel_cmd = Twist() - vel_cmd.linear.x = self.actions[action][0] - vel_cmd.angular.z = self.actions[action][1] - self.vel_pub.publish(vel_cmd) - - ##==== get image from sensor camera - f1_image_camera, _ = self.f1gazeboimages.get_camera_info() - self._gazebo_pause() - - ##==== get center - points_in_red_line, _ = self.simplifiedperception.processed_image( - f1_image_camera.data, self.height, self.width, self.x_row, self.center_image - ) - if self.state_space == "spn": - self.point = points_in_red_line[self.poi] - else: - self.point = points_in_red_line[0] - - center = abs(float(self.center_image - self.point) / (float(self.width) // 2)) - # center = float(self.center_image - self.point) / (float(self.width) // 2) - - ##==== get State - ##==== simplified perception as observation - state = self.simplifiedperception.calculate_observation( - points_in_red_line, self.center_image, self.pixel_region - ) - - ##==== get Rewards - if self.reward_function == "followline_center": - reward, done = self.f1gazeborewards.rewards_followline_center( - center, self.rewards - ) - - return state, reward, done, {} - - def step_followline_state_image_actions_continuous(self, action, step): - self._gazebo_unpause() - vel_cmd = Twist() - vel_cmd.linear.x = action[0][0] - vel_cmd.angular.z = action[0][1] - self.vel_pub.publish(vel_cmd) - - ##==== get image from sensor camera - f1_image_camera, _ = self.f1gazeboimages.get_camera_info() - self._gazebo_pause() - - ##==== get center - points_in_red_line, _ = self.simplifiedperception.processed_image( - f1_image_camera.data, self.height, self.width, self.x_row, self.center_image - ) - if self.state_space == "spn": - self.point = points_in_red_line[self.poi] - else: - self.point = points_in_red_line[0] - - center = abs(float(self.center_image - self.point) / (float(self.width) // 2)) - # center = float(self.center_image - self.point) / (float(self.width) // 2) - - ##==== get State - state = np.array( - self.f1gazeboimages.image_preprocessing_black_white_32x32( - f1_image_camera.data, self.height - ) - ) - - ##==== get Rewards - if self.reward_function == "followline_center": - reward, done = self.f1gazeborewards.rewards_followline_center( - center, self.rewards - ) - else: - reward, done = self.f1gazeborewards.rewards_followline_v_w_centerline( - vel_cmd, center, self.rewards, self.beta_1, self.beta_0 - ) - - return state, reward, done, {} - - def step_followline_state_sp_actions_continuous(self, action, step): - self._gazebo_unpause() - vel_cmd = Twist() - vel_cmd.linear.x = action[0][0] - vel_cmd.angular.z = action[0][1] - self.vel_pub.publish(vel_cmd) - - ##==== get image from sensor camera - f1_image_camera, _ = self.f1gazeboimages.get_camera_info() - self._gazebo_pause() - - ##==== get center - points_in_red_line, _ = self.simplifiedperception.processed_image( - f1_image_camera.data, self.height, self.width, self.x_row, self.center_image - ) - if self.state_space == "spn": - self.point = points_in_red_line[self.poi] - else: - self.point = points_in_red_line[0] - - center = abs(float(self.center_image - self.point) / (float(self.width) // 2)) - # center = float(self.center_image - self.point) / (float(self.width) // 2) - - ##==== get State - ##==== simplified perception as observation - state = self.simplifiedperception.calculate_observation( - points_in_red_line, self.center_image, self.pixel_region - ) - - ##==== get Rewards - if self.reward_function == "followline_center": - reward, done = self.f1gazeborewards.rewards_followline_center( - center, self.rewards - ) - else: - reward, done = self.f1gazeborewards.rewards_followline_v_w_centerline( - vel_cmd, center, self.rewards, self.beta_1, self.beta_0 - ) - - return state, reward, done, {} - - -class StepFollowLane(F1Env): - def __init__(self, **config): - self.name = config["states"] - - def step_followlane_state_sp_actions_discretes(self, action, step): - self._gazebo_unpause() - vel_cmd = Twist() - vel_cmd.linear.x = self.actions[action][0] - vel_cmd.angular.z = self.actions[action][1] - self.vel_pub.publish(vel_cmd) - - ##==== get image from sensor camera - f1_image_camera, _ = self.f1gazeboimages.get_camera_info() - self._gazebo_pause() - - ##==== get center - centrals_in_lane, centrals_in_lane_normalized = self.simplifiedperception.processed_image( - f1_image_camera.data, self.height, self.width, self.x_row, self.center_image - ) - if self.state_space == "spn": - self.point = centrals_in_lane[self.poi] - else: - self.point = centrals_in_lane[0] - - # center = abs(float(self.center_image - self.point) / (float(self.width) // 2)) - #center = float(self.center_image - self.point) / (float(self.width) // 2) - - #print(f"\n{centrals_in_lane = }") - #print(f"\n{centrals_in_lane_normalized = }") - #print(f"\n{self.point = }") - #print(f"\n{center = }") - - ##==== get State - ##==== simplified perception as observation - state = self.simplifiedperception.calculate_observation( - centrals_in_lane, self.center_image, self.pixel_region - ) - - ##==== get Rewards - if self.reward_function == "follow_right_lane_center_v_step": - reward, done = self.f1gazeborewards.rewards_followlane_v_centerline_step( - vel_cmd, centrals_in_lane_normalized[0], step, self.rewards - ) - else: - reward, done = self.f1gazeborewards.rewards_followlane_centerline( - centrals_in_lane_normalized[0], self.rewards - ) - - return state, reward, done, {} - - - - - - def step_followlane_state_image_actions_discretes(self, action, step): - self._gazebo_unpause() - vel_cmd = Twist() - vel_cmd.linear.x = self.actions[action][0] - vel_cmd.angular.z = self.actions[action][1] - self.vel_pub.publish(vel_cmd) - - ##==== get image from sensor camera - f1_image_camera, _ = self.f1gazeboimages.get_camera_info() - self._gazebo_pause() - - ##==== get center - points_in_red_line, _ = self.simplifiedperception.processed_image( - f1_image_camera.data, self.height, self.width, self.x_row, self.center_image - ) - if self.state_space == "spn": - self.point = points_in_red_line[self.poi] - else: - self.point = points_in_red_line[0] - - # center = abs(float(self.center_image - self.point) / (float(self.width) // 2)) - center = float(self.center_image - self.point) / (float(self.width) // 2) - - ##==== get State - ##==== image as observation - state = np.array( - self.f1gazeboimages.image_preprocessing_black_white_32x32( - f1_image_camera.data, self.height - ) - ) - - ##==== get Rewards - if self.reward_function == "follow_right_lane_center_v_step": - reward, done = self.f1gazeborewards.rewards_followlane_v_centerline_step( - vel_cmd, center, step, self.rewards - ) - else: - reward, done = self.f1gazeborewards.rewards_followlane_centerline( - center, self.rewards - ) - - return state, reward, done, {} - - def step_followlane_state_image_actions_continuous(self, action, step): - self._gazebo_unpause() - vel_cmd = Twist() - vel_cmd.linear.x = action[0][0] - vel_cmd.angular.z = action[0][1] - self.vel_pub.publish(vel_cmd) - - ##==== get image from sensor camera - f1_image_camera, _ = self.f1gazeboimages.get_camera_info() - self._gazebo_pause() - - ##==== get center - points_in_red_line, _ = self.simplifiedperception.processed_image( - f1_image_camera.data, self.height, self.width, self.x_row, self.center_image - ) - if self.state_space == "spn": - self.point = points_in_red_line[self.poi] - else: - self.point = points_in_red_line[0] - - # center = abs(float(self.center_image - self.point) / (float(self.width) // 2)) - center = float(self.center_image - self.point) / (float(self.width) // 2) - - ##==== get State - state = np.array( - self.f1gazeboimages.image_preprocessing_black_white_32x32( - f1_image_camera.data, self.height - ) - ) - - ##==== get Rewards - if self.reward_function == "follow_right_lane_center_v_step": - reward, done = self.f1gazeborewards.rewards_followlane_v_centerline_step( - vel_cmd, center, step, self.rewards - ) - else: - reward, done = self.f1gazeborewards.rewards_followlane_centerline( - center, self.rewards - ) - - return state, reward, done, {} - - def step_followlane_state_sp_actions_continuous(self, action, step): - self._gazebo_unpause() - vel_cmd = Twist() - vel_cmd.linear.x = action[0][0] - vel_cmd.angular.z = action[0][1] - self.vel_pub.publish(vel_cmd) - - ##==== get image from sensor camera - f1_image_camera, _ = self.f1gazeboimages.get_camera_info() - self._gazebo_pause() - - ##==== get center - points_in_red_line, _ = self.simplifiedperception.processed_image( - f1_image_camera.data, self.height, self.width, self.x_row, self.center_image - ) - if self.state_space == "spn": - self.point = points_in_red_line[self.poi] - else: - self.point = points_in_red_line[0] - - # center = abs(float(self.center_image - self.point) / (float(self.width) // 2)) - center = float(self.center_image - self.point) / (float(self.width) // 2) - - ##==== get State - ##==== simplified perception as observation - state = self.simplifiedperception.calculate_observation( - points_in_red_line, self.center_image, self.pixel_region - ) - - ##==== get Rewards - if self.reward_function == "follow_right_lane_center_v_step": - reward, done = self.f1gazeborewards.rewards_followlane_v_centerline_step( - vel_cmd, center, step, self.rewards - ) - else: - reward, done = self.f1gazeborewards.rewards_followlane_centerline( - center, self.rewards - ) - - return state, reward, done, {} diff --git a/rl_studio/envs/carla/followlane/utils.py b/rl_studio/envs/carla/followlane/utils.py index d4df1a3d4..889e9c33e 100644 --- a/rl_studio/envs/carla/followlane/utils.py +++ b/rl_studio/envs/carla/followlane/utils.py @@ -1,4 +1,5 @@ import cv2 +import imutils import numpy as np @@ -6,9 +7,460 @@ class AutoCarlaUtils: def __init__(self): self.f1 = None + @staticmethod + def finish_target(current_car_pose, target_pose, max_distance): + """ + working with waypoints + """ + current_car_pose_x = current_car_pose[0] + current_car_pose_y = current_car_pose[1] + + # in case working with waypoints, use next + target_x = target_pose.transform.location.x + target_y = target_pose.transform.location.y + + dist = ((current_car_pose_x - target_x) ** 2) + ( + (current_car_pose_y - target_y) ** 2 + ) + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + + # print(f"{current_car_pose = }") + # print(f"{target_x = }, {target_y = }") + # print(f"{dist = }") + + # print(dist) + if dist < max_distance: + return True, dist + return False, dist + + @staticmethod + def finish_fix_number_target( + current_car_pose, targets_set, target_pose, max_distance + ): + """ + working with tuple 0: [19.8, -238.2, 0.5, -4.5, -150.5, -0.5] + """ + current_car_pose_x = current_car_pose[0] + current_car_pose_y = current_car_pose[1] + + target_x = targets_set[target_pose][0] + target_y = targets_set[target_pose][1] + + dist = ((current_car_pose_x - target_x) ** 2) + ( + (current_car_pose_y - target_y) ** 2 + ) + dist = np.sum(dist, axis=0) + dist = np.sqrt(dist) + + # print(f"{current_car_pose = }") + # print(f"{target_x = }, {target_y = }") + # print(f"{dist = }") + + # print(dist) + if dist < max_distance: + return True, dist + return False, dist + + @staticmethod + def show_image_lines_centers_borders( + name, + img, + x_row, + x, + y, + index_right, + index_left, + centers, + drawing_lines_states, + drawing_numbers_states, + ): + """ + show image RGB with: + x_row lines + centers lanes + center image + lane borders + + """ + + window_name = f"{name}" + img = np.array(img) + + ## vertical line in the center of image, showing car position + cv2.line( + img, + (int(img.shape[1] // 2), int(img.shape[0] // 2)), + (int(img.shape[1] // 2), int(img.shape[0])), + # (320, 120), + # (320, 480), + color=(200, 100, 100), + thickness=4, + ) + + drawing_lines_states.append(640) + ## vertical lines for states: 5, 7, 8, 16... + for index, _ in enumerate(drawing_lines_states): + cv2.line( + img, + (drawing_lines_states[index], 0), + (drawing_lines_states[index], int(img.shape[0])), + color=(100, 200, 100), + thickness=1, + ) + ## writing number state into lines + cv2.putText( + img, + str(f"{drawing_numbers_states[index]}"), + ( + drawing_lines_states[index] - 15, + 15, + ), ## -15 is the distance to the its left line + cv2.FONT_HERSHEY_SIMPLEX, + 0.4, + (0, 0, 0), + 1, + cv2.LINE_AA, + ) + + for index, _ in enumerate(x_row): + ### HORIZONTAL LINES x_row + cv2.line( + img, + (0, int(x_row[index])), + (int(img.shape[1]), int(x_row[index])), + color=(100, 200, 100), + thickness=1, + ) + + ### Points + cv2.circle( + img, + (int(index_right[index]), x_row[index]), + 5, + # (150, 200, 150), + (0, 0, 255), + 2, + ) + cv2.circle( + img, + (int(index_left[index]), int(x_row[index])), + 4, + # (150, 200, 150), + (0, 0, 255), + 1, + ) + cv2.circle( + img, + (int(centers[index]), int(x_row[index])), + 4, + # (150, 200, 150), + (0, 0, 0), + 1, + ) + + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, img) + cv2.waitKey(1) + + @staticmethod + def show_image_only_right_line( + name, + img, + waitkey, + dist_in_pixels, + ground_truth_pixel_values, + dist_normalized, + states, + x_row, + x, + y, + line_states, + number_states, + ): + """ + shows image with 1 point in center of lane + """ + window_name = f"{name}" + img = np.array(img) + + ## vertical line in the center of image, showing car position + cv2.line( + img, + (int(img.shape[1] // 2), int(img.shape[0] // 2)), + (int(img.shape[1] // 2), int(img.shape[0])), + # (320, 120), + # (320, 480), + color=(200, 100, 100), + thickness=4, + ) + + line_states.append(640) + ## vertical lines for states: 5, 7, 8, 16... + for index, _ in enumerate(line_states): + cv2.line( + img, + (line_states[index], 0), + (line_states[index], int(img.shape[0])), + color=(100, 200, 100), + thickness=1, + ) + ## writing number state into lines + # for index, value in enumerate(number_states): + cv2.putText( + img, + str(f"{number_states[index]}"), + (line_states[index] - 30, 15), + cv2.FONT_HERSHEY_SIMPLEX, + 0.4, + (0, 0, 0), + 1, + cv2.LINE_AA, + ) + + for index, _ in enumerate(x_row): + ### Points + cv2.circle( + img, + ( + int(dist_in_pixels[index]), + int(x_row[index]), + ), + 5, + # (150, 200, 150), + (255, 255, 255), + 2, + ) + cv2.circle( + img, + (int(ground_truth_pixel_values[index]), int(x_row[index])), + 4, + # (150, 200, 150), + (255, 255, 255), + 1, + ) + + cv2.putText( + img, + str( + f"[right_line:{int(dist_in_pixels[index])}]-[dist:{dist_normalized[index]}]" + # f"[dist_norm:{int(centrals_in_pixels[index])}]-[state:{states[index]}]-[dist:{errors[index]}]" + ), + (int(dist_in_pixels[index]), int(x_row[index]) - 5), + cv2.FONT_HERSHEY_SIMPLEX, + 0.4, + # (255, 255, 255), + (255, 255, 255), + 1, + cv2.LINE_AA, + ) + + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, img) + cv2.waitKey(waitkey) + + @staticmethod + def show_image_with_three_points( + name, + img, + waitkey, + dist_in_pixels, + dist_normalized, + states, + x_row, + x, + y, + left_points, + right_points, + ): + """ + shows image with 1 point in center of lane + """ + window_name = f"{name}" + img = np.array(img) + + for index, _ in enumerate(x_row): + ### horizontal line in x_row file + cv2.line( + img, + (0, int(x_row[index])), + (int(img.shape[1]), int(x_row[index])), + color=(100, 200, 100), + thickness=1, + ) + ### vertical line in center of the image + cv2.line( + img, + (int(img.shape[1] // 2), 0), + (int(img.shape[1] // 2), int(img.shape[0])), + color=(100, 200, 100), + thickness=1, + ) + ### left limit vertical line (40%) + cv2.line( + img, + (int(img.shape[1] // 2) - (int((img.shape[1] // 2) * 0.4)), 0), + ( + int(img.shape[1] // 2) - (int((img.shape[1] // 2) * 0.4)), + int(img.shape[0]), + ), + color=(100, 200, 100), + thickness=1, + ) + ### right limit vertical line + cv2.line( + img, + (int(img.shape[1] // 2) + (int((img.shape[1] // 2) * 0.4)), 0), + ( + int(img.shape[1] // 2) + (int((img.shape[1] // 2) * 0.4)), + int(img.shape[0]), + ), + color=(100, 200, 100), + thickness=1, + ) + + ### Points + ## center point + cv2.circle( + img, + (int(dist_in_pixels[index]), int(x_row[index])), + 5, + # (150, 200, 150), + (255, 255, 255), + 2, + ) + # left point + cv2.circle( + img, + (int(left_points[index]), int(x_row[index])), + 5, + # (150, 200, 150), + (255, 255, 255), + 2, + ) + # right point + cv2.circle( + img, + (int(right_points[index]), int(x_row[index])), + 5, + # (150, 200, 150), + (255, 255, 255), + 2, + ) + + cv2.putText( + img, + str( + f"[right_line:{int(dist_in_pixels[index])}]-[state:{states[index]}]-[dist:{dist_normalized[index]}]" + # f"[dist_norm:{int(centrals_in_pixels[index])}]-[state:{states[index]}]-[dist:{errors[index]}]" + ), + (int(dist_in_pixels[index]), int(x_row[index]) - 5), + cv2.FONT_HERSHEY_SIMPLEX, + 0.4, + # (255, 255, 255), + (255, 255, 255), + 1, + cv2.LINE_AA, + ) + + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, img) + cv2.waitKey(waitkey) + + @staticmethod + def show_image_with_center_point( + name, img, waitkey, dist_in_pixels, dist_normalized, states, x_row, x, y + ): + """ + shows image with 1 point in center of lane + """ + window_name = f"{name}" + img = np.array(img) + + for index, _ in enumerate(x_row): + ### horizontal line in x_row file + cv2.line( + img, + (0, int(x_row[index])), + (int(img.shape[1]), int(x_row[index])), + color=(100, 200, 100), + thickness=1, + ) + ### vertical line in center of the image + cv2.line( + img, + (int(img.shape[1] // 2), 0), + (int(img.shape[1] // 2), int(img.shape[0])), + color=(100, 200, 100), + thickness=1, + ) + ### left limit vertical line (40%) + cv2.line( + img, + (int(img.shape[1] // 2) - (int((img.shape[1] // 2) * 0.4)), 0), + ( + int(img.shape[1] // 2) - (int((img.shape[1] // 2) * 0.4)), + int(img.shape[0]), + ), + color=(100, 200, 100), + thickness=1, + ) + ### right limit vertical line + cv2.line( + img, + (int(img.shape[1] // 2) + (int((img.shape[1] // 2) * 0.4)), 0), + ( + int(img.shape[1] // 2) + (int((img.shape[1] // 2) * 0.4)), + int(img.shape[0]), + ), + color=(100, 200, 100), + thickness=1, + ) + + ### Points + cv2.circle( + img, + ( + int(img.shape[1] // 2) + int(dist_in_pixels[index]), + int(x_row[index]), + ), + 5, + # (150, 200, 150), + (255, 255, 255), + 2, + ) + cv2.circle( + img, + (int(img.shape[1] // 2), int(x_row[index])), + 4, + # (150, 200, 150), + (255, 255, 255), + 1, + ) + + cv2.putText( + img, + str( + f"[right_line:{int(dist_in_pixels[index])}]-[state:{states[index]}]-[dist:{dist_normalized[index]}]" + # f"[dist_norm:{int(centrals_in_pixels[index])}]-[state:{states[index]}]-[dist:{errors[index]}]" + ), + (int(dist_in_pixels[index]), int(x_row[index]) - 5), + cv2.FONT_HERSHEY_SIMPLEX, + 0.4, + # (255, 255, 255), + (255, 255, 255), + 1, + cv2.LINE_AA, + ) + + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, img) + cv2.waitKey(waitkey) + @staticmethod def show_image_with_centrals( - name, img, waitkey, centrals_in_pixels, centrals_normalized, x_row + name, img, waitkey, centrals_in_pixels, centrals_normalized, x_row, x, y ): window_name = f"{name}" img = np.array(img) @@ -17,9 +469,13 @@ def show_image_with_centrals( cv2.putText( img, str(f"{int(centrals_in_pixels[index])}"), - (int(centrals_in_pixels[index]) + 20, int(x_row[index])), + ( + (img.shape[1] // 2) + int(centrals_in_pixels[index]) + 20, + int(x_row[index]), + ), cv2.FONT_HERSHEY_SIMPLEX, 0.3, + # (255, 255, 255), (255, 255, 255), 1, cv2.LINE_AA, @@ -27,9 +483,10 @@ def show_image_with_centrals( cv2.putText( img, str(f"[{centrals_normalized[index]}]"), - (320, int(x_row[index])), + (img.shape[1] - 100, int(x_row[index])), cv2.FONT_HERSHEY_SIMPLEX, 0.3, + # (255, 255, 255), (255, 255, 255), 1, cv2.LINE_AA, @@ -38,22 +495,75 @@ def show_image_with_centrals( img, (0, int(x_row[index])), (int(img.shape[1]), int(x_row[index])), - color=(10, 10, 10), + color=(100, 200, 100), thickness=1, ) cv2.line( img, (int(img.shape[1] // 2), 0), (int(img.shape[1] // 2), int(img.shape[0])), - color=(30, 30, 30), + color=(100, 200, 100), thickness=1, ) + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) cv2.imshow(window_name, img) cv2.waitKey(waitkey) @staticmethod - def show_image(name, img, waitkey): + def show_image(name, img, x, y): window_name = f"{name}" + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) cv2.imshow(window_name, img) - cv2.waitKey(waitkey) + cv2.waitKey(1) + + @staticmethod + def show_images(name, img, x, y): + window_name = f"{name}" + hori = np.concatenate(img, axis=1) + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, hori) + cv2.waitKey(1) + + @staticmethod + def show_images_tile(name, list_img, x, y): + window_name = f"{name}" + # img_tile = cv2.vconcat([cv2.hconcat(list_h) for list_h in list_img]) + + width_common = 320 + height_common = 240 + # image resizing + # im_list_resize = [ + # cv2.resize( + # np.array(img), + # (width_common, height_common), + # interpolation=cv2.INTER_CUBIC, + # ) + # for img in list_img + # ] + im_list_resize = [ + imutils.resize( + np.array(img), + width=width_common, + ) + for img in list_img + ] + # print(f"{im_list_resize = }") + # for i, value in enumerate(im_list_resize): + # print(f"({i = }") + # print(f"{len(im_list_resize) = }, {type(im_list_resize) = }") + im_list = [img for img in im_list_resize] + # print(f"{len(im_list) = }, {type(im_list) = }") + + im_list_concat = np.concatenate(im_list, axis=1) + # im_list_concat = cv2.hconcat( + # [im_list_resize[0], im_list_resize[1], im_list_resize[2]] + # ) + # im_list_concat = cv2.hconcat([im_list_resize[0], im_list_resize[1]]) + cv2.namedWindow(window_name) # Create a named window + cv2.moveWindow(window_name, x, y) # Move it to (40,30) + cv2.imshow(window_name, im_list_concat) + cv2.waitKey(1) diff --git a/rl_studio/envs/carla/utils/automatic_control_for_locations.py b/rl_studio/envs/carla/utils/automatic_control_for_locations.py new file mode 100755 index 000000000..7c7ce415e --- /dev/null +++ b/rl_studio/envs/carla/utils/automatic_control_for_locations.py @@ -0,0 +1,1025 @@ +#!/usr/bin/env python + +# Copyright (c) 2018 Intel Labs. +# authors: German Ros (german.ros@intel.com) +# +# This work is licensed under the terms of the MIT license. +# For a copy, see . + +"""Example of automatic vehicle control from client side.""" + +from __future__ import print_function + +import argparse +import collections +import datetime +import glob +import logging +import math +import os +import numpy.random as random +import re +import sys +import weakref + +try: + import pygame + from pygame.locals import KMOD_CTRL + from pygame.locals import K_ESCAPE + from pygame.locals import K_q +except ImportError: + raise RuntimeError("cannot import pygame, make sure pygame package is installed") + +try: + import numpy as np +except ImportError: + raise RuntimeError("cannot import numpy, make sure numpy package is installed") + +# ============================================================================== +# -- Find CARLA module --------------------------------------------------------- +# ============================================================================== +try: + sys.path.append( + glob.glob( + "../carla/dist/carla-*%d.%d-%s.egg" + % ( + sys.version_info.major, + sys.version_info.minor, + "win-amd64" if os.name == "nt" else "linux-x86_64", + ) + )[0] + ) +except IndexError: + pass + +# ============================================================================== +# -- Add PythonAPI for release mode -------------------------------------------- +# ============================================================================== +try: + sys.path.append( + os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + "/carla" + ) +except IndexError: + pass + +import carla +from carla import ColorConverter as cc + +from agents.navigation.behavior_agent import ( + BehaviorAgent, +) # pylint: disable=import-error +from agents.navigation.basic_agent import BasicAgent # pylint: disable=import-error + + +# ============================================================================== +# -- Global functions ---------------------------------------------------------- +# ============================================================================== + + +def find_weather_presets(): + """Method to find weather presets""" + rgx = re.compile(".+?(?:(?<=[a-z])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])|$)") + + def name(x): + return " ".join(m.group(0) for m in rgx.finditer(x)) + + presets = [x for x in dir(carla.WeatherParameters) if re.match("[A-Z].+", x)] + return [(getattr(carla.WeatherParameters, x), name(x)) for x in presets] + + +def get_actor_display_name(actor, truncate=250): + """Method to get actor display name""" + name = " ".join(actor.type_id.replace("_", ".").title().split(".")[1:]) + return (name[: truncate - 1] + "\u2026") if len(name) > truncate else name + + +# ============================================================================== +# -- World --------------------------------------------------------------- +# ============================================================================== + + +class World(object): + """Class representing the surrounding environment""" + + def __init__(self, carla_world, hud, args): + """Constructor method""" + self._args = args + self.world = carla_world + try: + self.map = self.world.get_map() + except RuntimeError as error: + print("RuntimeError: {}".format(error)) + print(" The server could not send the OpenDRIVE (.xodr) file:") + print( + " Make sure it exists, has the same name of your town, and is correct." + ) + sys.exit(1) + self.hud = hud + self.player = None + self.collision_sensor = None + self.lane_invasion_sensor = None + self.gnss_sensor = None + self.camera_manager = None + self._weather_presets = find_weather_presets() + self._weather_index = 0 + self._actor_filter = args.filter + self.restart(args) + self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False + self.recording_start = 0 + + def restart(self, args): + """Restart the world""" + # Keep same camera config if the camera manager exists. + cam_index = self.camera_manager.index if self.camera_manager is not None else 0 + cam_pos_id = ( + self.camera_manager.transform_index + if self.camera_manager is not None + else 0 + ) + + # Get a random blueprint. + blueprint = random.choice( + self.world.get_blueprint_library().filter(self._actor_filter) + ) + blueprint.set_attribute("role_name", "hero") + if blueprint.has_attribute("color"): + color = random.choice(blueprint.get_attribute("color").recommended_values) + blueprint.set_attribute("color", color) + + ################################### + # TODO: here we mark init position + + # Spawn the player. + if self.player is not None: + # spawn_point = self.player.get_transform() + # spawn_point.location.z += 2.0 + # spawn_point.rotation.roll = 0.0 + # spawn_point.rotation.pitch = 0.0 + location = carla.Transform( + carla.Location( + x=73.7, + y=-10, + z=0.3, + ), + carla.Rotation( + pitch=0.0, + yaw=-62.5, + roll=0.0, + ), + ) + + self.destroy() + self.player = self.world.spawn_actor(blueprint, location) + + self.modify_vehicle_physics(self.player) + + while self.player is None: + if not self.map.get_spawn_points(): + print("There are no spawn points available in your map/town.") + print("Please add some Vehicle Spawn Point to your UE4 scene.") + sys.exit(1) + + # TODO: init pose + location = carla.Transform( + carla.Location( + x=73.7, + y=-10, + z=0.3, + ), + carla.Rotation( + pitch=0.0, + yaw=-62.5, + roll=0.0, + ), + ) + + self.player = self.world.spawn_actor(blueprint, location) + self.modify_vehicle_physics(self.player) + + if self._args.sync: + self.world.tick() + else: + self.world.wait_for_tick() + + # Set up the sensors. + self.collision_sensor = CollisionSensor(self.player, self.hud) + self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) + self.gnss_sensor = GnssSensor(self.player) + self.camera_manager = CameraManager(self.player, self.hud) + self.camera_manager.transform_index = cam_pos_id + self.camera_manager.set_sensor(cam_index, notify=False) + actor_type = get_actor_display_name(self.player) + self.hud.notification(actor_type) + + def next_weather(self, reverse=False): + """Get next weather setting""" + self._weather_index += -1 if reverse else 1 + self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] + self.hud.notification("Weather: %s" % preset[1]) + self.player.get_world().set_weather(preset[0]) + + def modify_vehicle_physics(self, actor): + # If actor is not a vehicle, we cannot use the physics control + try: + physics_control = actor.get_physics_control() + physics_control.use_sweep_wheel_collision = True + actor.apply_physics_control(physics_control) + except Exception: + pass + + def tick(self, clock): + """Method for every tick""" + self.hud.tick(self, clock) + + def render(self, display): + """Render world""" + self.camera_manager.render(display) + self.hud.render(display) + + def destroy_sensors(self): + """Destroy sensors""" + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None + + def destroy(self): + """Destroys all actors""" + actors = [ + self.camera_manager.sensor, + self.collision_sensor.sensor, + self.lane_invasion_sensor.sensor, + self.gnss_sensor.sensor, + self.player, + ] + for actor in actors: + if actor is not None: + actor.destroy() + + +# ============================================================================== +# -- KeyboardControl ----------------------------------------------------------- +# ============================================================================== + + +class KeyboardControl(object): + def __init__(self, world): + world.hud.notification("Press 'H' or '?' for help.", seconds=4.0) + + def parse_events(self): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + if event.type == pygame.KEYUP: + if self._is_quit_shortcut(event.key): + return True + + @staticmethod + def _is_quit_shortcut(key): + """Shortcut for quitting""" + return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL) + + +# ============================================================================== +# -- HUD ----------------------------------------------------------------------- +# ============================================================================== + + +class HUD(object): + """Class for HUD text""" + + def __init__(self, width, height): + """Constructor method""" + self.dim = (width, height) + font = pygame.font.Font(pygame.font.get_default_font(), 20) + font_name = "courier" if os.name == "nt" else "mono" + fonts = [x for x in pygame.font.get_fonts() if font_name in x] + default_font = "ubuntumono" + mono = default_font if default_font in fonts else fonts[0] + mono = pygame.font.match_font(mono) + self._font_mono = pygame.font.Font(mono, 12 if os.name == "nt" else 14) + self._notifications = FadingText(font, (width, 40), (0, height - 40)) + self.help = HelpText(pygame.font.Font(mono, 24), width, height) + self.server_fps = 0 + self.frame = 0 + self.simulation_time = 0 + self._show_info = True + self._info_text = [] + self._server_clock = pygame.time.Clock() + + # TODO: init pose + self.x_prev, self.y_prev, self.z_prev = 73.7, -10, 0.3 + self.dist = 0 + + def on_world_tick(self, timestamp): + """Gets informations from the world at every tick""" + self._server_clock.tick() + self.server_fps = self._server_clock.get_fps() + self.frame = timestamp.frame_count + self.simulation_time = timestamp.elapsed_seconds + + def tick(self, world, clock): + """HUD method for every tick""" + self._notifications.tick(world, clock) + if not self._show_info: + return + transform = world.player.get_transform() + vel = world.player.get_velocity() + control = world.player.get_control() + heading = "N" if abs(transform.rotation.yaw) < 89.5 else "" + heading += "S" if abs(transform.rotation.yaw) > 90.5 else "" + heading += "E" if 179.5 > transform.rotation.yaw > 0.5 else "" + heading += "W" if -0.5 > transform.rotation.yaw > -179.5 else "" + colhist = world.collision_sensor.get_collision_history() + collision = [colhist[x + self.frame - 200] for x in range(0, 200)] + max_col = max(1.0, max(collision)) + collision = [x / max_col for x in collision] + vehicles = world.world.get_actors().filter("vehicle.*") + + self._info_text = [ + "Server: % 16.0f FPS" % self.server_fps, + "Client: % 16.0f FPS" % clock.get_fps(), + "", + "Vehicle: % 20s" % get_actor_display_name(world.player, truncate=20), + "Map: % 20s" % world.map.name.split("/")[-1], + "Simulation time: % 12s" + % datetime.timedelta(seconds=int(self.simulation_time)), + "", + "Speed: % 15.0f km/h" + % (3.6 * math.sqrt(vel.x**2 + vel.y**2 + vel.z**2)), + "Heading:% 16.0f\N{DEGREE SIGN} % 2s" % (transform.rotation.yaw, heading), + "Location:% 20s" + % ("(% 5.1f, % 5.1f)" % (transform.location.x, transform.location.y)), + "GNSS:% 24s" + % ("(% 2.6f, % 3.6f)" % (world.gnss_sensor.lat, world.gnss_sensor.lon)), + "Height: % 18.0f m" % transform.location.z, + "", + ] + if isinstance(control, carla.VehicleControl): + self._info_text += [ + ("Throttle:", control.throttle, 0.0, 1.0), + ("Steer:", control.steer, -1.0, 1.0), + ("Brake:", control.brake, 0.0, 1.0), + ("Reverse:", control.reverse), + ("Hand brake:", control.hand_brake), + ("Manual:", control.manual_gear_shift), + "Gear: %s" % {-1: "R", 0: "N"}.get(control.gear, control.gear), + ] + elif isinstance(control, carla.WalkerControl): + self._info_text += [ + ("Speed:", control.speed, 0.0, 5.556), + ("Jump:", control.jump), + ] + self._info_text += [ + "", + "Collision:", + collision, + "", + "Number of vehicles: % 8d" % len(vehicles), + ] + + if len(vehicles) > 1: + self._info_text += ["Nearby vehicles:"] + + def dist(l): + return math.sqrt( + (l.x - transform.location.x) ** 2 + + (l.y - transform.location.y) ** 2 + + (l.z - transform.location.z) ** 2 + ) + + vehicles = [ + (dist(x.get_location()), x) for x in vehicles if x.id != world.player.id + ] + + for dist, vehicle in sorted(vehicles): + if dist > 200.0: + break + vehicle_type = get_actor_display_name(vehicle, truncate=22) + self._info_text.append("% 4dm %s" % (dist, vehicle_type)) + + ############################ distance traveled + # nothing todo + + # print(f"{self.x_prev=}, {self.y_prev=}, {self.z_prev=}") + x_act, y_act, z_act = ( + transform.location.x, + transform.location.y, + transform.location.z, + ) + # print(f"{x_act=}, {y_act=}, {z_act=}") + self.dist += math.sqrt( + (x_act - self.x_prev) ** 2 + + (y_act - self.y_prev) ** 2 + + (z_act - self.z_prev) ** 2 + ) + # print(f"{self.dist =}") + self.x_prev, self.y_prev, self.z_prev = x_act, y_act, z_act + + def toggle_info(self): + """Toggle info on or off""" + self._show_info = not self._show_info + + def notification(self, text, seconds=2.0): + """Notification text""" + self._notifications.set_text(text, seconds=seconds) + + def error(self, text): + """Error text""" + self._notifications.set_text("Error: %s" % text, (255, 0, 0)) + + def render(self, display): + """Render for HUD class""" + if self._show_info: + info_surface = pygame.Surface((220, self.dim[1])) + info_surface.set_alpha(100) + display.blit(info_surface, (0, 0)) + v_offset = 4 + bar_h_offset = 100 + bar_width = 106 + for item in self._info_text: + if v_offset + 18 > self.dim[1]: + break + if isinstance(item, list): + if len(item) > 1: + points = [ + (x + 8, v_offset + 8 + (1 - y) * 30) + for x, y in enumerate(item) + ] + pygame.draw.lines(display, (255, 136, 0), False, points, 2) + item = None + v_offset += 18 + elif isinstance(item, tuple): + if isinstance(item[1], bool): + rect = pygame.Rect((bar_h_offset, v_offset + 8), (6, 6)) + pygame.draw.rect( + display, (255, 255, 255), rect, 0 if item[1] else 1 + ) + else: + rect_border = pygame.Rect( + (bar_h_offset, v_offset + 8), (bar_width, 6) + ) + pygame.draw.rect(display, (255, 255, 255), rect_border, 1) + fig = (item[1] - item[2]) / (item[3] - item[2]) + if item[2] < 0.0: + rect = pygame.Rect( + (bar_h_offset + fig * (bar_width - 6), v_offset + 8), + (6, 6), + ) + else: + rect = pygame.Rect( + (bar_h_offset, v_offset + 8), (fig * bar_width, 6) + ) + pygame.draw.rect(display, (255, 255, 255), rect) + item = item[0] + if item: # At this point has to be a str. + surface = self._font_mono.render(item, True, (255, 255, 255)) + display.blit(surface, (8, v_offset)) + v_offset += 18 + self._notifications.render(display) + self.help.render(display) + + +# ============================================================================== +# -- FadingText ---------------------------------------------------------------- +# ============================================================================== + + +class FadingText(object): + """Class for fading text""" + + def __init__(self, font, dim, pos): + """Constructor method""" + self.font = font + self.dim = dim + self.pos = pos + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + + def set_text(self, text, color=(255, 255, 255), seconds=2.0): + """Set fading text""" + text_texture = self.font.render(text, True, color) + self.surface = pygame.Surface(self.dim) + self.seconds_left = seconds + self.surface.fill((0, 0, 0, 0)) + self.surface.blit(text_texture, (10, 11)) + + def tick(self, _, clock): + """Fading text method for every tick""" + delta_seconds = 1e-3 * clock.get_time() + self.seconds_left = max(0.0, self.seconds_left - delta_seconds) + self.surface.set_alpha(500.0 * self.seconds_left) + + def render(self, display): + """Render fading text method""" + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- HelpText ------------------------------------------------------------------ +# ============================================================================== + + +class HelpText(object): + """Helper class for text render""" + + def __init__(self, font, width, height): + """Constructor method""" + lines = __doc__.split("\n") + self.font = font + self.dim = (680, len(lines) * 22 + 12) + self.pos = (0.5 * width - 0.5 * self.dim[0], 0.5 * height - 0.5 * self.dim[1]) + self.seconds_left = 0 + self.surface = pygame.Surface(self.dim) + self.surface.fill((0, 0, 0, 0)) + for i, line in enumerate(lines): + text_texture = self.font.render(line, True, (255, 255, 255)) + self.surface.blit(text_texture, (22, i * 22)) + self._render = False + self.surface.set_alpha(220) + + def toggle(self): + """Toggle on or off the render help""" + self._render = not self._render + + def render(self, display): + """Render help text method""" + if self._render: + display.blit(self.surface, self.pos) + + +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + """Class for collision sensors""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + self.sensor = None + self.history = [] + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + blueprint = world.get_blueprint_library().find("sensor.other.collision") + self.sensor = world.spawn_actor( + blueprint, carla.Transform(), attach_to=self._parent + ) + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda event: CollisionSensor._on_collision(weak_self, event) + ) + + def get_collision_history(self): + """Gets the history of collisions""" + history = collections.defaultdict(int) + for frame, intensity in self.history: + history[frame] += intensity + return history + + @staticmethod + def _on_collision(weak_self, event): + """On collision method""" + self = weak_self() + if not self: + return + actor_type = get_actor_display_name(event.other_actor) + self.hud.notification("Collision with %r" % actor_type) + impulse = event.normal_impulse + intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) + self.history.append((event.frame, intensity)) + if len(self.history) > 4000: + self.history.pop(0) + + +# ============================================================================== +# -- LaneInvasionSensor -------------------------------------------------------- +# ============================================================================== + + +class LaneInvasionSensor(object): + """Class for lane invasion sensors""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + self.sensor = None + self._parent = parent_actor + self.hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find("sensor.other.lane_invasion") + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda event: LaneInvasionSensor._on_invasion(weak_self, event) + ) + + @staticmethod + def _on_invasion(weak_self, event): + """On invasion method""" + self = weak_self() + if not self: + return + lane_types = set(x.type for x in event.crossed_lane_markings) + text = ["%r" % str(x).split()[-1] for x in lane_types] + self.hud.notification("Crossed line %s" % " and ".join(text)) + + +# ============================================================================== +# -- GnssSensor -------------------------------------------------------- +# ============================================================================== + + +class GnssSensor(object): + """Class for GNSS sensors""" + + def __init__(self, parent_actor): + """Constructor method""" + self.sensor = None + self._parent = parent_actor + self.lat = 0.0 + self.lon = 0.0 + world = self._parent.get_world() + blueprint = world.get_blueprint_library().find("sensor.other.gnss") + self.sensor = world.spawn_actor( + blueprint, + carla.Transform(carla.Location(x=1.0, z=2.8)), + attach_to=self._parent, + ) + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event)) + + @staticmethod + def _on_gnss_event(weak_self, event): + """GNSS method""" + self = weak_self() + if not self: + return + self.lat = event.latitude + self.lon = event.longitude + + +# ============================================================================== +# -- CameraManager ------------------------------------------------------------- +# ============================================================================== + + +class CameraManager(object): + """Class for camera management""" + + def __init__(self, parent_actor, hud): + """Constructor method""" + self.sensor = None + self.surface = None + self._parent = parent_actor + self.hud = hud + self.recording = False + bound_y = 0.5 + self._parent.bounding_box.extent.y + attachment = carla.AttachmentType + self._camera_transforms = [ + ( + carla.Transform( + carla.Location(x=-5.5, z=2.5), carla.Rotation(pitch=8.0) + ), + attachment.SpringArm, + ), + (carla.Transform(carla.Location(x=1.6, z=1.7)), attachment.Rigid), + ( + carla.Transform(carla.Location(x=5.5, y=1.5, z=1.5)), + attachment.SpringArm, + ), + ( + carla.Transform( + carla.Location(x=-8.0, z=6.0), carla.Rotation(pitch=6.0) + ), + attachment.SpringArm, + ), + ( + carla.Transform(carla.Location(x=-1, y=-bound_y, z=0.5)), + attachment.Rigid, + ), + ] + self.transform_index = 1 + self.sensors = [ + ["sensor.camera.rgb", cc.Raw, "Camera RGB"], + ["sensor.camera.depth", cc.Raw, "Camera Depth (Raw)"], + ["sensor.camera.depth", cc.Depth, "Camera Depth (Gray Scale)"], + [ + "sensor.camera.depth", + cc.LogarithmicDepth, + "Camera Depth (Logarithmic Gray Scale)", + ], + [ + "sensor.camera.semantic_segmentation", + cc.Raw, + "Camera Semantic Segmentation (Raw)", + ], + [ + "sensor.camera.semantic_segmentation", + cc.CityScapesPalette, + "Camera Semantic Segmentation (CityScapes Palette)", + ], + ["sensor.lidar.ray_cast", None, "Lidar (Ray-Cast)"], + ] + world = self._parent.get_world() + bp_library = world.get_blueprint_library() + for item in self.sensors: + blp = bp_library.find(item[0]) + if item[0].startswith("sensor.camera"): + blp.set_attribute("image_size_x", str(hud.dim[0])) + blp.set_attribute("image_size_y", str(hud.dim[1])) + elif item[0].startswith("sensor.lidar"): + blp.set_attribute("range", "50") + item.append(blp) + self.index = None + + def toggle_camera(self): + """Activate a camera""" + self.transform_index = (self.transform_index + 1) % len(self._camera_transforms) + self.set_sensor(self.index, notify=False, force_respawn=True) + + def set_sensor(self, index, notify=True, force_respawn=False): + """Set a sensor""" + index = index % len(self.sensors) + needs_respawn = ( + True + if self.index is None + else ( + force_respawn or (self.sensors[index][0] != self.sensors[self.index][0]) + ) + ) + if needs_respawn: + if self.sensor is not None: + self.sensor.destroy() + self.surface = None + self.sensor = self._parent.get_world().spawn_actor( + self.sensors[index][-1], + self._camera_transforms[self.transform_index][0], + attach_to=self._parent, + attachment_type=self._camera_transforms[self.transform_index][1], + ) + + # We need to pass the lambda a weak reference to + # self to avoid circular reference. + weak_self = weakref.ref(self) + self.sensor.listen( + lambda image: CameraManager._parse_image(weak_self, image) + ) + if notify: + self.hud.notification(self.sensors[index][2]) + self.index = index + + def next_sensor(self): + """Get the next sensor""" + self.set_sensor(self.index + 1) + + def toggle_recording(self): + """Toggle recording on or off""" + self.recording = not self.recording + self.hud.notification("Recording %s" % ("On" if self.recording else "Off")) + + def render(self, display): + """Render method""" + if self.surface is not None: + display.blit(self.surface, (0, 0)) + + @staticmethod + def _parse_image(weak_self, image): + self = weak_self() + if not self: + return + if self.sensors[self.index][0].startswith("sensor.lidar"): + points = np.frombuffer(image.raw_data, dtype=np.dtype("f4")) + points = np.reshape(points, (int(points.shape[0] / 4), 4)) + lidar_data = np.array(points[:, :2]) + lidar_data *= min(self.hud.dim) / 100.0 + lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1]) + lidar_data = np.fabs( + lidar_data + ) # pylint: disable=assignment-from-no-return + lidar_data = lidar_data.astype(np.int32) + lidar_data = np.reshape(lidar_data, (-1, 2)) + lidar_img_size = (self.hud.dim[0], self.hud.dim[1], 3) + lidar_img = np.zeros(lidar_img_size) + lidar_img[tuple(lidar_data.T)] = (255, 255, 255) + self.surface = pygame.surfarray.make_surface(lidar_img) + else: + image.convert(self.sensors[self.index][1]) + array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) + if self.recording: + image.save_to_disk("_out/%08d" % image.frame) + + +# ============================================================================== +# -- Game Loop --------------------------------------------------------- +# ============================================================================== + + +def game_loop(args): + """ + Main loop of the simulation. It handles updating all the HUD information, + ticking the agent and, if needed, the world. + """ + + pygame.init() + pygame.font.init() + world = None + + try: + if args.seed: + random.seed(args.seed) + + client = carla.Client(args.host, args.port) + client.set_timeout(4.0) + + traffic_manager = client.get_trafficmanager() + # sim_world = client.get_world() + + # TODO: load town + sim_world = client.load_world("Town07") + + if args.sync: + settings = sim_world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + sim_world.apply_settings(settings) + + traffic_manager.set_synchronous_mode(True) + + display = pygame.display.set_mode( + (args.width, args.height), pygame.HWSURFACE | pygame.DOUBLEBUF + ) + + hud = HUD(args.width, args.height) + world = World(client.get_world(), hud, args) + controller = KeyboardControl(world) + if args.agent == "Basic": + agent = BasicAgent(world.player) + else: + agent = BehaviorAgent(world.player, behavior=args.behavior) + + # Set the agent destination + spawn_points = world.map.get_spawn_points() + destination = random.choice(spawn_points).location + # print(f"{destination.x = }") + + # TODO: target destination in x, y, z coordinates + destination.x = 23.7 # 19.8 + destination.y = -236.2 # -239 + destination.z = 0.5 # -239 + + agent.set_destination(destination) + + clock = pygame.time.Clock() + + while True: + clock.tick() + if args.sync: + world.world.tick() + else: + world.world.wait_for_tick() + if controller.parse_events(): + return + + world.tick(clock) + world.render(display) + pygame.display.flip() + + if agent.done(): + if args.loop: + agent.set_destination(random.choice(spawn_points).location) + world.hud.notification( + "The target has been reached, searching for another target", + seconds=4.0, + ) + print("The target has been reached, searching for another target") + else: + print("The target has been reached, stopping the simulation") + break + + control = agent.run_step() + control.manual_gear_shift = False + world.player.apply_control(control) + + finally: + print(f"distance total : {hud.dist=}") + if world is not None: + settings = world.world.get_settings() + settings.synchronous_mode = False + settings.fixed_delta_seconds = None + world.world.apply_settings(settings) + traffic_manager.set_synchronous_mode(True) + + world.destroy() + + pygame.quit() + + +# ============================================================================== +# -- main() -------------------------------------------------------------- +# ============================================================================== + + +def main(): + """Main method""" + + argparser = argparse.ArgumentParser(description="CARLA Automatic Control Client") + argparser.add_argument( + "-v", + "--verbose", + action="store_true", + dest="debug", + help="Print debug information", + ) + argparser.add_argument( + "--host", + metavar="H", + default="127.0.0.1", + help="IP of the host server (default: 127.0.0.1)", + ) + argparser.add_argument( + "-p", + "--port", + metavar="P", + default=2000, + type=int, + help="TCP port to listen to (default: 2000)", + ) + argparser.add_argument( + "--res", + metavar="WIDTHxHEIGHT", + default="1280x720", + help="Window resolution (default: 1280x720)", + ) + argparser.add_argument( + "--sync", action="store_true", help="Synchronous mode execution" + ) + argparser.add_argument( + "--filter", + metavar="PATTERN", + default="vehicle.*", + help='Actor filter (default: "vehicle.*")', + ) + argparser.add_argument( + "-l", + "--loop", + action="store_true", + dest="loop", + help="Sets a new random destination upon reaching the previous one (default: False)", + ) + argparser.add_argument( + "-a", + "--agent", + type=str, + choices=["Behavior", "Basic"], + help="select which agent to run", + default="Behavior", + ) + argparser.add_argument( + "-b", + "--behavior", + type=str, + choices=["cautious", "normal", "aggressive"], + help="Choose one of the possible agent behaviors (default: normal) ", + default="normal", + ) + argparser.add_argument( + "-s", + "--seed", + help="Set seed for repeating executions (default: None)", + default=None, + type=int, + ) + + args = argparser.parse_args() + + args.width, args.height = [int(x) for x in args.res.split("x")] + + log_level = logging.DEBUG if args.debug else logging.INFO + logging.basicConfig(format="%(levelname)s: %(message)s", level=log_level) + + logging.info("listening to server %s:%s", args.host, args.port) + + print(__doc__) + + try: + game_loop(args) + + except KeyboardInterrupt: + print("\nCancelled by user. Bye!") + + +if __name__ == "__main__": + main() diff --git a/rl_studio/envs/carla/utils/visualize_multiple_sensors.py b/rl_studio/envs/carla/utils/visualize_multiple_sensors.py index 9ddabf54a..e140cd85b 100644 --- a/rl_studio/envs/carla/utils/visualize_multiple_sensors.py +++ b/rl_studio/envs/carla/utils/visualize_multiple_sensors.py @@ -342,4 +342,5 @@ def render(self): def destroy(self): # print(f"in DisplayManeger.destroy - self.sensor = {self.sensor}") + print(f"destroying actor {self.sensor}") self.sensor.destroy() diff --git a/rl_studio/models/fastai_torch_lane_detector_model.pth b/rl_studio/models/fastai_torch_lane_detector_model.pth new file mode 100644 index 000000000..b834beaa5 Binary files /dev/null and b/rl_studio/models/fastai_torch_lane_detector_model.pth differ