From 5bf599f5ccacd229fc1c61c5e7461949992c90f7 Mon Sep 17 00:00:00 2001 From: emadmosayyebi Date: Thu, 8 Jun 2023 23:36:10 +0400 Subject: [PATCH 1/2] A2mosayyebi-golabadi --- src/__main__.py | 42 +++++++++++++++++--------- src/path_following_handler.py | 36 +++++++++++++++++------ src/simulator_handler.py | 55 +++++++++++++++++++++++------------ 3 files changed, 92 insertions(+), 41 deletions(-) diff --git a/src/__main__.py b/src/__main__.py index fdb8552..b83448e 100644 --- a/src/__main__.py +++ b/src/__main__.py @@ -4,27 +4,43 @@ from src.simulator_handler import SimulatorHandler from utils.vehicle_command import VehicleCommand +from path_following_handler import PathFollowingHandler +import warnings + +warnings.filterwarnings("ignore") if __name__ == "__main__": - simulator_handler = SimulatorHandler(town_name="Town04") - simulator_handler.spawn_vehicle(spawn_index=13) - simulator_handler.set_weather(weather=carla.WeatherParameters.ClearNoon) + simulator_handler = SimulatorHandler(town_name="Town10HD_Opt") - # potential weather choices are [ClearNoon, ClearSunset, CloudyNoon, CloudySunset, - # WetNoon, WetSunset, MidRainyNoon, MidRainSunset, HardRainNoon, HardRainSunset, - # SoftRainNoon, SoftRainSunset] + simulator_handler.set_weather(weather=carla.WeatherParameters.ClearNoon) - # add sensors + # potential weather choices we have [ ClearSunset,CloudySunset,CloudyNoon,ClearNoon] + # WetNoon, WetSunset,HardRainNoon,HardRainSunset,MidRainyNoon,SoftRainNoon, SoftRainSunset] + path_following_handler = PathFollowingHandler(client=simulator_handler.client) + ego_spawn_point = path_following_handler.ego_spawn_point + ego_vehicle = simulator_handler.spawn_ego_vehicles(road_id=ego_spawn_point["road_id"], + filtered_points_index=ego_spawn_point["filtered_points_index"]) + # sensors we have: rgb_cam = simulator_handler.rgb_cam() gnss_sensor = simulator_handler.gnss() imu_sensor = simulator_handler.imu() - # listen to sensor data + # listen to sensor data: rgb_cam.listen(lambda image: simulator_handler.rgb_cam_callback(image)) imu_sensor.listen(lambda imu: simulator_handler.imu_callback(imu)) gnss_sensor.listen(lambda gnss: simulator_handler.gnss_callback(gnss)) - VehicleCommand(throttle=1.0).send_control(simulator_handler.vehicle) - time.sleep(20.0) - - - + # VehicleCommand(throttle=1.0).send_control(simulator_handler.vehicle) + + if path_following_handler.debug_mode: + path_following_handler.start() + else: + # ego_vehicle = path_following_handler.spawn_ego_vehicles(road_id=ego_spawn_point["road_id"], + # filtered_points_index=ego_spawn_point["filtered_points_index"]) + ego_pid_controller = path_following_handler.pid_controller(ego_vehicle, + path_following_handler.pid_values_lateral, + path_following_handler.pid_values_longitudinal) + path_following_handler.vehicle_and_controller_inputs(ego_vehicle, ego_pid_controller) + path_following_handler.start() + + # time.sleep(20.0) + # simulator_handler.clearing() diff --git a/src/path_following_handler.py b/src/path_following_handler.py index b7a75c6..f894029 100644 --- a/src/path_following_handler.py +++ b/src/path_following_handler.py @@ -9,6 +9,7 @@ from src.utils.carla_utils import draw_waypoints, filter_waypoints, TrajectoryToFollow, InfiniteLoopThread from src.utils.controller import VehiclePIDController +# from src.simulator_handler import SimulatorHandler class PathFollowingHandler(InfiniteLoopThread, ABC): @@ -26,6 +27,7 @@ def __init__(self, client: carla.Client, debug_mode: bool = False, {'road_id': road_id_list, 'filtered_points_index': filtered_point_index_list} self.world: World = self.client.get_world() + self.spectator = self.world.get_spectator() if os.path.basename(self.world.get_map().name) != carla_map: self.world: World = client.load_world(carla_map) self.waypoints: list = self.client.get_world().get_map().generate_waypoints(distance=1.0) @@ -34,15 +36,15 @@ def __init__(self, client: carla.Client, debug_mode: bool = False, self.waypoints_to_visualize = {'road_id': [4], 'filtered_points_index': [0]} self.pid_values_lateral: Union[Dict[str, float], Dict[str, float], Dict[str, float]] = \ {'K_P': 1, - 'K_D': 0.07, - 'K_I': 0.05} # control steering + 'K_D': 0.09, + 'K_I': 0.04} #steering control self.pid_values_longitudinal: Union[Dict[str, float], Dict[str, float], Dict[str, float]] = \ - {'K_P': 1, - 'K_D': 0.07, - 'K_I': 0.05} # control speed + {'K_P': 1.5, + 'K_D': 0.09, + 'K_I': 0.08} #speed control self.vehicle_to_target_distance_threshold: float = 2.5 - self.desired_speed: int = 20 # meter per second + self.desired_speed: int = 20 #meter per second self.reached_destination: bool = False self.previous_waypoint: Union[carla.Waypoint, None] = None @@ -93,6 +95,7 @@ def pid_controller(vehicle, args_lateral: dict, args_longitudinal: dict) -> Vehi return ego_pid_controller_ def follow_trajectory(self, vehicle: Any, ego_pid_controller_: VehiclePIDController) -> None: + for trajectory_point_index in range(len(self.trajectory_to_follow['road_id'])): current_road_id, current_filtered_point_index = \ self.trajectory_to_follow['road_id'][trajectory_point_index], \ @@ -117,6 +120,20 @@ def follow_trajectory(self, vehicle: Any, ego_pid_controller_: VehiclePIDControl self.__follow_target_waypoints__(vehicle, target_waypoint, ego_pid_controller_) self.previous_waypoint = target_waypoint + def infinite_trajectory_tracking(self, vehicle, ego_pid_controller_): + curr_waypoint_index = {'road_id': 13, 'filtered_points_index': 0} #get first way point + curr_waypoint = filter_waypoints(self.waypoints, + road_id=curr_waypoint_index['road_id'])[curr_waypoint_index['filtered_points_index']] + print(curr_waypoint) + while True: + way_points = curr_waypoint.next_until_lane_end(2) + draw_waypoints(self.world, self.waypoints, road_id=curr_waypoint.road_id , life_time=30) + for target_waypoint in way_points: + self.__follow_target_waypoints__(vehicle, target_waypoint, ego_pid_controller_) + curr_waypoint = target_waypoint.next(3.5)[0] + + + def vehicle_and_controller_inputs(self, ego_vehicle_, ego_pid_controller_): self.ego_vehicle = ego_vehicle_ self.ego_pid_controller = ego_pid_controller_ @@ -128,9 +145,10 @@ def __step__(self): sys.exit(1) if not self.reached_destination: - self.follow_trajectory(self.ego_vehicle, self.ego_pid_controller) - self.reached_destination = True - print("Destination has been reached.") + # self.follow_trajectory(self.ego_vehicle, self.ego_pid_controller) + self.infinite_trajectory_tracking(self.ego_vehicle, self.ego_pid_controller) + # self.reached_destination = True + # print("Destination has been reached.") else: sys.exit(1) diff --git a/src/simulator_handler.py b/src/simulator_handler.py index ba3b914..a3fff29 100644 --- a/src/simulator_handler.py +++ b/src/simulator_handler.py @@ -5,9 +5,12 @@ import numpy as np import pandas as pd from matplotlib import pyplot as plt +from src.path_following_handler import PathFollowingHandler +from src.utils.carla_utils import draw_waypoints, filter_waypoints, TrajectoryToFollow, InfiniteLoopThread +from typing import Any, Union, Dict, List -class SimulatorHandler: +class SimulatorHandler(): def __init__(self, town_name): self.spawn_point = None self.vehicle = None @@ -23,29 +26,42 @@ def __init__(self, town_name): try: print("Trying to communicate with the client...") - client = carla.Client("localhost", 2000) - client.set_timeout(8.0) - self.world = client.get_world() + self.client = carla.Client("localhost", 2000) + self.client.set_timeout(8.0) + self.world = self.client.get_world() if os.path.basename(self.world.get_map().name) != town_name: - self.world: carla.World = client.load_world(town_name) + self.world: carla.World = self.client.load_world(town_name) self.blueprint_library = self.world.get_blueprint_library() self.actor_list = [] self.vehicle_list = [] - self.IM_WIDTH = 800 # Ideally a config file should be defined for such parameters - self.IM_HEIGHT = 600 + self.IM_WIDTH = 1280 # Ideally a config file should be defined for such parameters + self.IM_HEIGHT = 720 print("Successfully connected to CARLA client") except Exception as error: raise Exception(f"Error while initializing the simulator: {error}") self.imu_dataframe = pd.DataFrame({}) self.gnss_dataframe = pd.DataFrame({}) - - def spawn_vehicle(self, spawn_index: int = 90): - self.vehicle_blueprint = self.blueprint_library.filter("model3")[0] # choosing the car - self.spawn_point = self.world.get_map().get_spawn_points()[spawn_index] - self.vehicle = self.world.spawn_actor(self.vehicle_blueprint, self.spawn_point) + self.waypoints: list = self.client.get_world().get_map().generate_waypoints(distance=1.0) + + # def spawn_vehicle(self, spawn_index: int = 90): + # self.vehicle_blueprint = self.blueprint_library.filter("model3")[0] # selecting the car + # self.spawn_point = self.world.get_map().get_spawn_points()[spawn_index] + # self.vehicle = self.world.spawn_actor(self.vehicle_blueprint, self.spawn_point) + # self.actor_list.append(self.vehicle) + + def spawn_ego_vehicles(self, road_id: int, filtered_points_index: int) -> Any: + print("spawning ego vehicle at road_id={} filtered_points_index={}".format(road_id, + filtered_points_index)) + vehicle_blueprint = \ + self.client.get_world().get_blueprint_library().filter("model3")[0] + filtered_waypoints = filter_waypoints(self.waypoints, road_id=road_id) + spawn_point = filtered_waypoints[filtered_points_index].transform + spawn_point.location.z += 2 + self.vehicle = self.client.get_world().spawn_actor(vehicle_blueprint, spawn_point) self.actor_list.append(self.vehicle) + return self.vehicle def set_weather(self, weather=carla.WeatherParameters.ClearNoon): self.world.set_weather(weather) @@ -90,11 +106,6 @@ def rgb_cam_callback(self, image): rgba_image = raw_image.reshape((self.IM_HEIGHT, self.IM_WIDTH, 4)) # because carla rgb cam is rgba rgb_image = rgba_image[:, :, :3] - rgb_image = cv2.cvtColor(rgb_image, cv2.COLOR_BGR2RGB) - plt.imshow(rgb_image) - plt.show() - # cv2.imshow("rgb camera", rgb_image) # FixMe: Replace with pygame visualization - # cv2.waitKey(1) def imu_callback(self, imu): # accelerometer is m/s^2 and gyroscope data is rad/sec imu_dict = {} @@ -108,7 +119,7 @@ def imu_callback(self, imu): # accelerometer is m/s^2 and gyroscope data is rad # create a pandas dataframe self.imu_dataframe = self.imu_dataframe.append(imu_dict, ignore_index=True) # save the dataframe to a csv file - self.imu_dataframe.to_csv(os.path.join(self.save_dir, "imu.csv"), index=False) + self.imu_dataframe.to_csv(os.path.join(self.save_dir, "imu.csv"), index=False, mode='a') def gnss_callback(self, gnss): gnss_dict = {} @@ -117,4 +128,10 @@ def gnss_callback(self, gnss): gnss_dict["longitude"] = gnss.longitude gnss_dict["altitude"] = gnss.altitude self.gnss_dataframe = self.gnss_dataframe.append(gnss_dict, ignore_index=True) - self.gnss_dataframe.to_csv(os.path.join(self.save_dir, "gnss.csv"), index=False) + self.gnss_dataframe.to_csv(os.path.join(self.save_dir, "gnss.csv"), index=False, mode='a') + + def clearing(self): + for actor in self.world.get_actors().filter('*vehicle*'): + actor.destroy() + for actor in self.world.get_actors().filter('*sensor*'): + actor.destroy() From 56b40332ba11d2efb3ada6701fc0cf7dd646c9f7 Mon Sep 17 00:00:00 2001 From: emadmosayyebi Date: Thu, 8 Jun 2023 23:56:02 +0400 Subject: [PATCH 2/2] A2mosayyebi401822045-golabadi400634087 --- .idea/.gitignore | 3 +++ .idea/CARLA_tutorialA2mosayyebi-golabadi.iml | 12 ++++++++++++ .idea/inspectionProfiles/profiles_settings.xml | 6 ++++++ .idea/misc.xml | 4 ++++ .idea/modules.xml | 8 ++++++++ .idea/vcs.xml | 6 ++++++ 6 files changed, 39 insertions(+) create mode 100644 .idea/.gitignore create mode 100644 .idea/CARLA_tutorialA2mosayyebi-golabadi.iml create mode 100644 .idea/inspectionProfiles/profiles_settings.xml create mode 100644 .idea/misc.xml create mode 100644 .idea/modules.xml create mode 100644 .idea/vcs.xml diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/.idea/CARLA_tutorialA2mosayyebi-golabadi.iml b/.idea/CARLA_tutorialA2mosayyebi-golabadi.iml new file mode 100644 index 0000000..8b8c395 --- /dev/null +++ b/.idea/CARLA_tutorialA2mosayyebi-golabadi.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..d56657a --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..e5ec441 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..35eb1dd --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file