diff --git a/src/__main__.py b/src/__main__.py index fdb8552..505fae4 100644 --- a/src/__main__.py +++ b/src/__main__.py @@ -1,30 +1,66 @@ -import time +import os +import sys +import glob +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 + +sys.path.append(r"D:\Term8\CARLA_0.9.8\WindowsNoEditor\PythonAPI\carla\dist\carla-0.9.8-py3.7-win-amd64.egg") import carla +import time + +sys.path.append(r"D:\CARLA_tutorial-main\src") +sys.path.append(r"D:\CARLA_tutorial-main\src\utils") +sys.path.append(r"D:\CARLA_tutorial-main\utils") -from src.simulator_handler import SimulatorHandler -from utils.vehicle_command import VehicleCommand +from simulator_handler import SimulatorHandler +from vehicle_command import VehicleCommand +from path_following_handler import PathFollowingHandler if __name__ == "__main__": - simulator_handler = SimulatorHandler(town_name="Town04") - simulator_handler.spawn_vehicle(spawn_index=13) + simulator_handler = SimulatorHandler(town_name="Town01") simulator_handler.set_weather(weather=carla.WeatherParameters.ClearNoon) # potential weather choices are [ClearNoon, ClearSunset, CloudyNoon, CloudySunset, # WetNoon, WetSunset, MidRainyNoon, MidRainSunset, HardRainNoon, HardRainSunset, # 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"]) + + # add sensors rgb_cam = simulator_handler.rgb_cam() gnss_sensor = simulator_handler.gnss() imu_sensor = simulator_handler.imu() + LIDAR_sensor = simulator_handler.LIDAR() + radar_sensor = simulator_handler.radar() + collision_sensor = simulator_handler.collision() # 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) - + LIDAR_sensor.listen(lambda LIDAR: simulator_handler.LIDAR_callback(LIDAR)) + radar_sensor.listen(lambda radar: simulator_handler.radar_callback(radar)) + collision_sensor.listen(lambda collision: simulator_handler.collision_callback(collision)) + # VehicleCommand(throttle=1.0).send_control(simulator_handler.vehicle) + # time.sleep(20.0) + if path_following_handler.debug_mode: + path_following_handler.start() + else: + 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() diff --git a/src/path_following_handler.py b/src/path_following_handler.py index b7a75c6..a1d5613 100644 --- a/src/path_following_handler.py +++ b/src/path_following_handler.py @@ -1,14 +1,27 @@ import math import os import sys +import glob +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 + +sys.path.append(r"D:\Term8\CARLA_0.9.8\WindowsNoEditor\PythonAPI\carla\dist\carla-0.9.8-py3.7-win-amd64.egg") +sys.path.append(r"D:\CARLA_tutorial-main\src") +sys.path.append(r"D:\CARLA_tutorial-main\src\utils") + from abc import ABC from typing import Any, Union, Dict, List import carla from carla import World -from src.utils.carla_utils import draw_waypoints, filter_waypoints, TrajectoryToFollow, InfiniteLoopThread -from src.utils.controller import VehiclePIDController +from utils.carla_utils import draw_waypoints, filter_waypoints, TrajectoryToFollow, InfiniteLoopThread +from utils.controller import VehiclePIDController class PathFollowingHandler(InfiniteLoopThread, ABC): @@ -26,6 +39,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) @@ -33,16 +47,16 @@ def __init__(self, client: carla.Client, debug_mode: bool = False, if self.debug_mode: 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_P': 0.8, + 'K_D': 0.09, + 'K_I': 0.04} # control steering 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 self.vehicle_to_target_distance_threshold: float = 2.5 - self.desired_speed: int = 20 # meter per second + self.desired_speed: int = 22 # meter per second self.reached_destination: bool = False self.previous_waypoint: Union[carla.Waypoint, None] = None @@ -93,34 +107,48 @@ 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], \ - self.trajectory_to_follow['filtered_points_index'][trajectory_point_index] - draw_waypoints(self.world, self.waypoints, road_id=current_road_id, life_time=30) - print("Following point: {}/{}".format(trajectory_point_index, - len(self.trajectory_to_follow['road_id']) - 1)) - print('current_road_id: {}, current_filtered_point_index: {}'.format(current_road_id, - current_filtered_point_index)) - if current_road_id == 1000: # 1000 means using waypoint.next - target_waypoint = self.previous_waypoint.next(float( - current_filtered_point_index))[0] - elif current_road_id == 2000: # 2000 means using waypoint.next_until_lane_end - target_waypoints: List[carla.Waypoint] = self.previous_waypoint.next_until_lane_end(float( - current_filtered_point_index)) - for target_waypoint in target_waypoints: - self.__follow_target_waypoints__(vehicle, target_waypoint, ego_pid_controller_) - self.previous_waypoint = target_waypoint - else: - filtered_waypoints = filter_waypoints(self.waypoints, road_id=current_road_id) - target_waypoint = filtered_waypoints[current_filtered_point_index] - self.__follow_target_waypoints__(vehicle, target_waypoint, ego_pid_controller_) - self.previous_waypoint = target_waypoint + while True: + 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], \ + self.trajectory_to_follow['filtered_points_index'][trajectory_point_index] + draw_waypoints(self.world, self.waypoints, road_id=current_road_id, life_time=30) + print("Following point: {}/{}".format(trajectory_point_index, + len(self.trajectory_to_follow['road_id']) - 1)) + print('current_road_id: {}, current_filtered_point_index: {}'.format(current_road_id, + current_filtered_point_index)) + if current_road_id == 1000: # 1000 means using waypoint.next + target_waypoint = self.previous_waypoint.next(float( + current_filtered_point_index))[0] + elif current_road_id == 2000: # 2000 means using waypoint.next_until_lane_end + target_waypoints: List[carla.Waypoint] = self.previous_waypoint.next_until_lane_end(float( + current_filtered_point_index)) + for target_waypoint in target_waypoints: + self.__follow_target_waypoints__(vehicle, target_waypoint, ego_pid_controller_) + self.previous_waypoint = target_waypoint + else: + filtered_waypoints = filter_waypoints(self.waypoints, road_id=current_road_id) + target_waypoint = filtered_waypoints[current_filtered_point_index] + self.__follow_target_waypoints__(vehicle, target_waypoint, ego_pid_controller_) + self.previous_waypoint = target_waypoint + def vehicle_and_controller_inputs(self, ego_vehicle_, ego_pid_controller_): self.ego_vehicle = ego_vehicle_ self.ego_pid_controller = ego_pid_controller_ + def infinite_trajectory_tracking(self, vehicle, ego_pid_controller_): + waypoint_index_2 = {'road_id': 1, 'filtered_points_index': 0} + waypoint_2 = filter_waypoints(self.waypoints, + road_id=waypoint_index_2['road_id'])[waypoint_index_2['filtered_points_index']] + print(waypoint_2) + while True: + way_points = waypoint_2.next_until_lane_end(2) + draw_waypoints(self.world, self.waypoints, road_id=waypoint_2.road_id , life_time=30) + for target_waypoint in way_points: + self.__follow_target_waypoints__(vehicle, target_waypoint, ego_pid_controller_) + waypoint_2 = target_waypoint.next(3.5)[0] + def __step__(self): if self.debug_mode: self.visualize_road_id(road_id=self.waypoints_to_visualize['road_id'][0], @@ -128,9 +156,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.reached_destination = True + # print("Destination has been reached.") + self.infinite_trajectory_tracking(self.ego_vehicle, self.ego_pid_controller) else: sys.exit(1) diff --git a/src/simulator_handler.py b/src/simulator_handler.py index ba3b914..5eabb46 100644 --- a/src/simulator_handler.py +++ b/src/simulator_handler.py @@ -1,11 +1,26 @@ +import glob import os +import sys + +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 + +sys.path.append(r"D:\CARLA_tutorial-main\src") +sys.path.append(r"D:\CARLA_tutorial-main\src\utils") import carla import cv2 import numpy as np import pandas as pd from matplotlib import pyplot as plt - +from path_following_handler import PathFollowingHandler +from utils.carla_utils import draw_waypoints, filter_waypoints, TrajectoryToFollow, InfiniteLoopThread +from typing import Any, Union, Dict, List class SimulatorHandler: def __init__(self, town_name): @@ -23,11 +38,11 @@ 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 = [] @@ -40,12 +55,36 @@ def __init__(self, town_name): 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.LIDAR_dataframe = pd.DataFrame({}) + self.radar_dataframe = pd.DataFrame({}) + self.collision_dataframe = pd.DataFrame({}) + + for actor in self.world.get_actors().filter('*vehicle*'): + actor.destroy() + for actor in self.world.get_actors().filter('*sensor*'): + actor.destroy() + + 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] # 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.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 = 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) @@ -83,16 +122,51 @@ def imu(self): attachment_type=carla.AttachmentType.Rigid) self.actor_list.append(ego_imu) return ego_imu - + + def LIDAR(self): + LIDAR_sensor = self.blueprint_library.find("sensor.lidar.ray_cast") + LIDAR_sensor.set_attribute('range', '100.0') + LIDAR_sensor.set_attribute('upper_fov', '15.0') + LIDAR_sensor.set_attribute('lower_fov', '-25.0') + LIDAR_sensor.set_attribute('channels', '64.0') + LIDAR_sensor.set_attribute('rotation_frequency', '20.0') + LIDAR_sensor.set_attribute('points_per_second', '500000') + + LIDAR_transform = carla.Transform(carla.Location(z=2)) + ego_LIDAR = self.world.spawn_actor(LIDAR_sensor, LIDAR_transform, attach_to=self.vehicle, + attachment_type=carla.AttachmentType.Rigid) + self.actor_list.append(ego_LIDAR) + return ego_LIDAR + + def radar(self): + radar_sensor = self.blueprint_library.find("sensor.other.radar") + radar_sensor.set_attribute('horizontal_fov', str(30)) + radar_sensor.set_attribute('vertical_fov', str(30)) + radar_sensor.set_attribute('points_per_second', '10000') + radar_transform = carla.Transform(carla.Location(z=2)) + ego_radar = self.world.spawn_actor(radar_sensor, radar_transform, attach_to=self.vehicle, + attachment_type=carla.AttachmentType.Rigid) + self.actor_list.append(ego_radar) + return ego_radar + + def collision(self): + collision_sensor = self.blueprint_library.find('sensor.other.collision') + collision_location = carla.Location(0, 0, 0) + collision_rotation = carla.Rotation(0, 0, 0) + collision_transform = carla.Transform(collision_location, collision_rotation) + ego_collision =self.world.spawn_actor(collision_sensor, collision_transform, attach_to=self.vehicle, + attachment_type=carla.AttachmentType.Rigid) + self.actor_list.append(ego_collision) + return ego_collision def rgb_cam_callback(self, image): image.save_to_disk("data/rgb_cam/%06d.jpg" % image.frame) - raw_image = np.array(image.raw_data) + # raw_image = np.array(image.raw_data) - 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() + # 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) @@ -118,3 +192,30 @@ def gnss_callback(self, gnss): 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) + def collision_callback(self, collision): + collision_dict = {} + collision_dict["Collision Time"] = collision.timestamp + collision_dict["collision_actor"] = collision.other_actor # The actor collided with + collision_dict["collision_impulse"] = collision.normal_impulse # The impulse of the hit + self.collision_dataframe = self.collision_dataframe.append(collision_dict, ignore_index=True) + self.collision_dataframe.to_csv(os.path.join(self.save_dir, "collision.csv"), index=False) + + def LIDAR_callback(self, LIDAR): + # LIDAR_dict = {} + points = np.frombuffer(LIDAR.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (len(LIDAR), 3)) + # LIDAR_dict["timestamp"] = LIDAR.timestamp + # LIDAR_dict["horizontal_angle"] = LIDAR.horizontal_angle + # LIDAR_dict["raw_data"] = LIDAR.raw_data + # LIDAR_dict["location"] = LIDAR.transform.location + # LIDAR_dict["rotation"] = LIDAR.transform.rotation + df = pd.DataFrame(points, columns=["X", "Y", "Z"]) + self.LIDAR_dataframe = self.LIDAR_dataframe.append(df, ignore_index=True) + self.LIDAR_dataframe.to_csv(os.path.join(self.save_dir, "LIDAR.csv"), index=False) + + def radar_callback(self,radar): + points = np.frombuffer(radar.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (len(radar), 4)) + df = pd.DataFrame(points, columns=["altitude","azimuth","depth","velocity"]) + self.radar_dataframe = self.radar_dataframe.append(df, ignore_index=True) + self.radar_dataframe.to_csv(os.path.join(self.save_dir, "radar.csv"), index=False) \ No newline at end of file diff --git a/src/utils/carla_utils.py b/src/utils/carla_utils.py index 6297424..44dd038 100644 --- a/src/utils/carla_utils.py +++ b/src/utils/carla_utils.py @@ -27,11 +27,12 @@ def __init__(self, trajectory_index: int) -> None: def get_trajectory_data(self) -> Tuple[Any, list, list]: if self.trajectory_index == 0: - carla_map = "Town10HD_Opt" - road_id_list: list = [13, 12, 12, 11, 2, 10] - filtered_point_index_list: list = [-4, -1, 11, 1, 0, 0] + carla_map = "Town01" + road_id_list: list = [1, 2000, 1000, 25, 2000, 10, 2000, 1000, + 16,2000,1000] + filtered_point_index_list: list = [0,1,5,0,1,0,1,8,-1,1,5] elif self.trajectory_index == 1: - carla_map = "Town10HD_Opt" + carla_map = "Town04" road_id_list: list = [1, 2000, 1, 2, 1000, 21, 20, 20, 1000, 15, 22, 6, 2000] filtered_point_index_list: list = [-300, 1, -4, -4, 25, -1, -1, 3, 10, 0, 3, -1, 1] else: @@ -41,7 +42,7 @@ def get_trajectory_data(self) -> Tuple[Any, list, list]: def get_ego_vehicle_spwan_point(self) -> Union[Dict[str, int], Dict[str, int]]: if self.trajectory_index == 0: - ego_spawn_point: Union = {'road_id': 13, 'filtered_points_index': 0} + ego_spawn_point: Union = {'road_id': 1, 'filtered_points_index': 0} elif self.trajectory_index == 1: ego_spawn_point: Union = {'road_id': 1, 'filtered_points_index': 4} else: