diff --git a/src/__main__.py b/src/__main__.py index fdb8552..66f8916 100644 --- a/src/__main__.py +++ b/src/__main__.py @@ -1,30 +1,102 @@ +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_0.9.8_2\WindowsNoEditor\PythonAPI\carla\dist\carla-0.9.8-py3.7-win-amd64.egg") + +sys.path.append(r"D:\CARLA_Code\integrate two session\src") +from utils.carla_utils import draw_waypoints, filter_waypoints, TrajectoryToFollow, InfiniteLoopThread + import time import carla -from src.simulator_handler import SimulatorHandler -from utils.vehicle_command import VehicleCommand +sys.path.append(r"D:\CARLA_Code\integrate two session\src") +sys.path.append(r"D:\CARLA_Code\integrate two session\utils") + +from simulator_handler import SimulatorHandler +from path_following_handler import PathFollowingHandler +from vehicle_command import VehicleCommand + +if __name__ == '__main__': + client = carla.Client("localhost", 2000) + client.set_timeout(8.0) + + town_name = "Town05" + # spawn_index = 2 + + try: + print("Trying to communicate with the client...") + world = client.get_world() + if os.path.basename(world.get_map().name) != town_name: + world: carla.World = client.load_world(town_name) + + blueprint_library = world.get_blueprint_library() + actor_list = [] + print("Successfully connected to CARLA client") + except Exception as error: + raise Exception(f"Error while initializing the simulator: {error}") + + simulator_handler = SimulatorHandler(client=client, actor_list=actor_list) + + weather = [carla.WeatherParameters(cloudiness=20.0, sun_altitude_angle=90.0, fog_density=0.0), # day + carla.WeatherParameters(cloudiness=20.0, sun_altitude_angle=-90.0, fog_density=0.0), # night + carla.WeatherParameters(cloudiness=20.0, sun_altitude_angle=90.0, fog_density=60.0), # fog + carla.WeatherParameters(cloudiness=85.0, sun_altitude_angle=90.0, fog_density=0.0)] # cloud + + world.set_weather(weather[3]) + + # weather = carla.WeatherParameters(cloudiness=100.0,sun_altitude_angle=165.0,fog_density=0.0) + # world.set_weather(weather) + + # carla.WeatherParameters(cloudiness=20.0, + # sun_altitude_angle=100.0,fog_density=60.0) + + path_following_handler = PathFollowingHandler(client=client, debug_mode=False) + + vehicle_blueprint = blueprint_library.filter("model3")[0] # choosing the car + # spawn_point = world.get_map().get_spawn_points()[spawn_index] + # vehicle = world.spawn_actor(vehicle_blueprint, spawn_point) + + ego_spawn_point = path_following_handler.ego_spawn_point -if __name__ == "__main__": - simulator_handler = SimulatorHandler(town_name="Town04") - simulator_handler.spawn_vehicle(spawn_index=13) - simulator_handler.set_weather(weather=carla.WeatherParameters.ClearNoon) + filtered_waypoints = filter_waypoints(path_following_handler.waypoints, road_id=ego_spawn_point["road_id"]) + spawn_point = filtered_waypoints[ego_spawn_point["filtered_points_index"]].transform + spawn_point.location.z += 2 + vehicle = client.get_world().spawn_actor(vehicle_blueprint, spawn_point) + actor_list.append(vehicle) - # potential weather choices are [ClearNoon, ClearSunset, CloudyNoon, CloudySunset, - # WetNoon, WetSunset, MidRainyNoon, MidRainSunset, HardRainNoon, HardRainSunset, - # SoftRainNoon, SoftRainSunset] + rgb_cam = simulator_handler.rgb_cam(vehicle) - # add sensors - rgb_cam = simulator_handler.rgb_cam() - gnss_sensor = simulator_handler.gnss() - imu_sensor = simulator_handler.imu() + # gnss_sensor = simulator_handler.gnss(vehicle) + # imu_sensor = simulator_handler.imu(vehicle) + # lidar = simulator_handler.lidar(vehicle) + # radar = simulator_handler.radar(vehicle) + # collision = simulator_handler.collision(vehicle) # 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) + # imu_sensor.listen(lambda imu: simulator_handler.imu_callback(imu)) + # gnss_sensor.listen(lambda gnss: simulator_handler.gnss_callback(gnss)) + # lidar.listen(lambda data: simulator_handler.lidar_callback(data)) + # radar.listen(lambda data: simulator_handler.radar_callback(data)) + # collision.listen(lambda event: simulator_handler.collision_callback(event)) + if path_following_handler.debug_mode: + path_following_handler.start() + else: + ego_pid_controller = path_following_handler.pid_controller(vehicle, + path_following_handler.pid_values_lateral, + path_following_handler.pid_values_longitudinal) + path_following_handler.vehicle_and_controller_inputs(vehicle, ego_pid_controller) + path_following_handler.start() diff --git a/src/adverse_weather_classification/train.py b/src/adverse_weather_classification/train.py index 8822b6b..94a3060 100644 --- a/src/adverse_weather_classification/train.py +++ b/src/adverse_weather_classification/train.py @@ -9,11 +9,17 @@ from tensorflow.keras.callbacks import ModelCheckpoint from mock import Mock import matplotlib.pyplot as plt +from sklearn.metrics import classification_report, confusion_matrix + +num_of_test_samples = 0 +for root_dir, cur_dir, files in os.walk(r"D:\CARLA_Code\trainSet\test"): + num_of_test_samples += len(files) +print('num_of_test_samples count:', num_of_test_samples) class TrainHyperParameters: - def __init__(self, input_shape: Tuple[int, int, int] = (256, 256, 3), number_of_classes: int = 2, - learning_rate: float = 0.001, batch_size: int = 32, number_of_epochs: int = 3) -> None: + def __init__(self, input_shape: Tuple[int, int, int] = (256, 256, 3), number_of_classes: int = 4, + learning_rate: float = 0.001, batch_size: int = 32, number_of_epochs: int = 5) -> None: self.hyperparameters = Mock() self.hyperparameters.input_shape = input_shape self.hyperparameters.number_of_classes = number_of_classes @@ -60,12 +66,23 @@ def model_builder(self): self.model = keras.models.Sequential([ keras.layers.Conv2D(32, (3, 3), activation='relu', input_shape=self.hyperparameters.input_shape), keras.layers.MaxPooling2D((2, 2)), + keras.layers.Dropout(0.2), + keras.layers.Conv2D(64, (3, 3), activation='relu'), keras.layers.Conv2D(64, (3, 3), activation='relu'), keras.layers.MaxPooling2D((2, 2)), + keras.layers.Dropout(0.2), + keras.layers.Conv2D(128, (3, 3), activation='relu'), keras.layers.Conv2D(128, (3, 3), activation='relu'), keras.layers.MaxPooling2D((2, 2)), + keras.layers.Dropout(0.2), + keras.layers.Conv2D(512, (3, 3), activation='relu'), + keras.layers.Conv2D(512, (3, 3), activation='relu'), + keras.layers.MaxPooling2D((2, 2)), + keras.layers.Dropout(0.2), keras.layers.Flatten(), - keras.layers.Dense(128, activation='relu'), + keras.layers.Dense(1024, activation='relu'), + keras.layers.Dropout(0.2), + keras.layers.Dense(64, activation='relu'), keras.layers.Dropout(0.5), keras.layers.Dense(self.hyperparameters.number_of_classes, activation='softmax') ]) @@ -106,6 +123,15 @@ def train(self, train_generator, test_generator): # plot loss and accuracy on train and validation set self.plot_history(history) + Y_pred = self.model.predict_generator(test_generator, + num_of_test_samples // self.hyperparameters.batch_size + 1) + y_pred = np.argmax(Y_pred, axis=1) + print('Confusion Matrix') + print(confusion_matrix(test_generator.classes, y_pred)) + print('Classification Report') + target_names = ['fog ', 'day', 'cloud', 'night'] + print(classification_report(test_generator.classes, y_pred, target_names=target_names)) + def plot_history(self, history): matplotlib.use('Agg') plt.figure(figsize=(10, 5)) @@ -128,6 +154,7 @@ def exec(self): if __name__ == '__main__': - data_dir_ = '/home/ahv/PycharmProjects/Visual-Inertial-Odometry/simulation/CARLA/output/root_dir' + data_dir_ = r"D:\CARLA_Code\trainSet" train_custom_cnn = TrainCustomCNN(data_dir_) train_custom_cnn.exec() + diff --git a/src/adverse_weather_classification/weather_classification.py b/src/adverse_weather_classification/weather_classification.py index 777ddfe..7feb2aa 100644 --- a/src/adverse_weather_classification/weather_classification.py +++ b/src/adverse_weather_classification/weather_classification.py @@ -12,7 +12,7 @@ def __init__(self, model_path, model_input_size: Tuple[int, int] = (256, 256)) - self.model = None self.model_path = model_path self.model_input_size = model_input_size - self.class_labels = ['day', 'night'] + self.class_labels = ['fog ', 'day','night','cloud'] def load(self): start_time = time.time() @@ -40,8 +40,8 @@ def exec(self, frame: np.ndarray) -> str: if __name__ == "__main__": - img_dir = "/home/ahv/PycharmProjects/Visual-Inertial-Odometry/simulation/CARLA/output/root_dir/testing_imgs" - model_path_ = "/src/adverse_weather_classification/output/checkpoints/best_model.h5" + img_dir = r"D:\CARLA_Code\trainSet\finalTest" + model_path_ = r"D:\CARLA_Code\output\checkpoints\best_model.h5" adverse_weather_classifier = AdverseWeatherClassifier(model_path_) adverse_weather_classifier.load() for root, dirs, files in os.walk(img_dir): diff --git a/src/path_following_handler.py b/src/path_following_handler.py index b7a75c6..1fd1e30 100644 --- a/src/path_following_handler.py +++ b/src/path_following_handler.py @@ -3,12 +3,18 @@ import sys from abc import ABC from typing import Any, Union, Dict, List +from datetime import datetime +import numpy as np + +sys.path.append(r"D:\CARLA_0.9.8_2\WindowsNoEditor\PythonAPI\carla\dist\carla-0.9.8-py3.7-win-amd64.egg") +sys.path.append(r"D:\CARLA_Code\integrate two session\src") +sys.path.append(r"D:\CARLA_Code\integrate two session\utils") 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): @@ -17,6 +23,7 @@ def __init__(self, client: carla.Client, debug_mode: bool = False, super(PathFollowingHandler, self).__init__(name="CARLA trajectory following handler") self.trajectory_to_follow_handler = TrajectoryToFollow(trajectory_index=trajectory_index) self.ego_vehicle = None + self.weatherCounter = 1; self.ego_pid_controller = None carla_map, road_id_list, filtered_point_index_list = self.trajectory_to_follow_handler.get_trajectory_data() self.ego_spawn_point: Union[Dict[str, int], Dict[str, int]] = \ @@ -31,10 +38,10 @@ def __init__(self, client: carla.Client, debug_mode: bool = False, self.waypoints: list = self.client.get_world().get_map().generate_waypoints(distance=1.0) self.debug_mode = debug_mode if self.debug_mode: - self.waypoints_to_visualize = {'road_id': [4], 'filtered_points_index': [0]} + self.waypoints_to_visualize = {'road_id': [1], '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_P': 2, + 'K_D': 0, 'K_I': 0.05} # control steering self.pid_values_longitudinal: Union[Dict[str, float], Dict[str, float], Dict[str, float]] = \ {'K_P': 1, @@ -42,17 +49,30 @@ def __init__(self, client: carla.Client, debug_mode: bool = False, '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 = 15 # meter per second self.reached_destination: bool = False self.previous_waypoint: Union[carla.Waypoint, None] = None + # self.i = 0 + self.intersectionCounter = 0 + # for x in range(4): + # with open("intersection"+str(x)+".csv", 'a') as csvfile: + # ww = np.array(["timeStamp","P,I,D", "Yaw"]) + # ww = np.reshape(ww, (1, 3)) + # np.savetxt(csvfile, ww, delimiter=',', fmt=['%s', '%s', '%s'], comments='') + def __follow_target_waypoints__(self, vehicle: Any, target_waypoint, ego_pid_controller_) -> None: self.client.get_world().debug.draw_string(target_waypoint.transform.location, 'O', draw_shadow=False, color=carla.Color(r=255, g=0, b=0), life_time=20, persistent_lines=True) + + # dict = {} + self.i = 0 + while True: + self.i = self.i + 1 vehicle_loc = vehicle.get_location() vehicle_to_target_distance = math.sqrt((target_waypoint.transform.location.x - vehicle_loc.x) ** 2 + (target_waypoint.transform.location.y - vehicle_loc.y) ** 2) @@ -63,6 +83,15 @@ def __follow_target_waypoints__(self, vehicle: Any, target_waypoint, ego_pid_con else: control_signal = ego_pid_controller_.run_step(desired_speed, target_waypoint) vehicle.apply_control(control_signal) + # print("***************", vehicle.get_transform().rotation.yaw) + + # if self.i % 500 == 0: + # with open("intersection"+str(self.intersectionCounter%4)+".csv", 'a') as csvfile: + # yaw = vehicle.get_transform().rotation.yaw + # PID = str(self.pid_values_lateral['K_P'])+" ,"+str(self.pid_values_lateral['K_I'])+" ,"+str(self.pid_values_lateral['K_D']) + # data = np.array([str(datetime.now()),PID, str(yaw)]) + # data = np.reshape(data, (1, 3)) + # np.savetxt(csvfile, data, delimiter=',', fmt=['%s', '%s', '%s'], comments='') def visualize_road_id(self, road_id: int, filtered_points_index: int, life_time: int = 5) -> None: # For debugging purposes. @@ -93,29 +122,41 @@ 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 + weather = [carla.WeatherParameters(cloudiness=20.0, sun_altitude_angle=90.0, fog_density=0.0), # day + carla.WeatherParameters(cloudiness=20.0, sun_altitude_angle=-90.0, fog_density=0.0), # night + carla.WeatherParameters(cloudiness=20.0, sun_altitude_angle=90.0, fog_density=60.0), # fog + carla.WeatherParameters(cloudiness=100.0, sun_altitude_angle=165.0, fog_density=0.0)] # cloud + 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 + self.intersectionCounter += 1 + if self.intersectionCounter % 4 == 0: + self.world.set_weather(weather[self.weatherCounter]) + self.weatherCounter += 1 + if (self.weatherCounter == 4): + self.weatherCounter = 0 + 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_ @@ -138,20 +179,21 @@ def terminate(self): print("Terminating trajectory following handler") self.join() - -if __name__ == '__main__': - client_ = carla.Client("localhost", 2000) - client_.set_timeout(8.0) - path_following_handler = PathFollowingHandler(client=client_, debug_mode=False) - ego_spawn_point = path_following_handler.ego_spawn_point - 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() +# if __name__ == '__main__': +# client_ = carla.Client("localhost", 2000) +# client_.set_timeout(8.0) +# path_following_handler = PathFollowingHandler(client=client_, debug_mode=False) +# ego_spawn_point = path_following_handler.ego_spawn_point +# 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() diff --git a/src/simulator_handler.py b/src/simulator_handler.py index ba3b914..8f2ad02 100644 --- a/src/simulator_handler.py +++ b/src/simulator_handler.py @@ -1,16 +1,27 @@ import os +import sys -import carla import cv2 import numpy as np import pandas as pd from matplotlib import pyplot as plt +from matplotlib import cm + +sys.path.append(r"D:\CARLA_0.9.8_2\WindowsNoEditor\PythonAPI\carla\dist\carla-0.9.8-py3.7-win-amd64.egg") +import carla +from carla import World + + +# sys.path.append(r"D:\CARLA_Code\integrate two session\src") class SimulatorHandler: - def __init__(self, town_name): + def __init__(self, client: carla.Client, actor_list): + self.raw_data = None + self.COOL = None + self.COOL_RANGE = None + self.viridis = None self.spawn_point = None - self.vehicle = None self.rgb_cam_sensor = None self.vehicle_blueprint = None @@ -21,36 +32,79 @@ def __init__(self, town_name): if not os.path.exists(os.path.join(self.save_dir, "rgb_cam")): os.makedirs(os.path.join(self.save_dir, "rgb_cam")) - try: - print("Trying to communicate with the client...") - client = carla.Client("localhost", 2000) - client.set_timeout(8.0) - self.world = client.get_world() - if os.path.basename(self.world.get_map().name) != town_name: - self.world: carla.World = 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 - 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.actor_list.append(self.vehicle) - - def set_weather(self, weather=carla.WeatherParameters.ClearNoon): - self.world.set_weather(weather) - - def rgb_cam(self): + self.client = client + self.world: World = self.client.get_world() + self.blueprint_library = self.world.get_blueprint_library() + + self.IM_WIDTH = 800 # Ideally a config file should be defined for such parameters + self.IM_HEIGHT = 600 + + self.actor_list = actor_list + + # Add LIDAR and RADAR, and collision sensors + + def lidar(self, vehicle): + lidar_sensor = self.blueprint_library.find("sensor.lidar.ray_cast") + # lidar_sensor.set_attribute("sensor_tick", str(0.0)) + # 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') + with open("src/data/lidar.csv", 'a') as csvfile: + ww = np.array(["X", "Y", "Z"]) + ww = np.reshape(ww, (1, 3)) + np.savetxt(csvfile, ww, delimiter=',', fmt=['%s', '%s', '%s'], comments='') + self.viridis = np.array(cm.get_cmap('plasma').colors) + lidar_location = carla.Location(0, 0, 0) + lidar_rotation = carla.Rotation(0, 0, 0) + lidar_transform = carla.Transform(lidar_location, lidar_rotation) + lidar_sen = self.world.spawn_actor(lidar_sensor, lidar_transform, attach_to=vehicle, + attachment_type=carla.AttachmentType.Rigid) + self.actor_list.append(lidar_sen) + return lidar_sen + + def radar(self, vehicle): + radar_sensor = self.blueprint_library.find("sensor.other.radar") + radar_sensor.set_attribute('horizontal_fov', '30.0') + radar_sensor.set_attribute('vertical_fov', '30.0') + radar_sensor.set_attribute('points_per_second', '10000') + with open('src/data/radar.csv', 'a') as csvfile: + ww = np.array(["altitude", "azimuth", "depth", "velocity"]) + ww = np.reshape(ww, (1, 4)) + np.savetxt(csvfile, ww, delimiter=',', fmt=['%s', '%s', '%s', '%s'], comments='') + self.viridis = np.array(cm.get_cmap('plasma').colors) + # VID_RANGE = np.linspace(0.0, 1.0, self.viridis.shape[0]) + self.COOL_RANGE = np.linspace(0.0, 1.0, self.viridis.shape[0]) + self.COOL = np.array(cm.get_cmap('winter')(self.COOL_RANGE)) + self.COOL = self.COOL[:, :3] + radar_location = carla.Location(0, 0, 0) + radar_rotation = carla.Rotation(0, 0, 0) + radar_transform = carla.Transform(radar_location, radar_rotation) + radar_sen = self.world.spawn_actor(radar_sensor, radar_transform, attach_to=vehicle, + attachment_type=carla.AttachmentType.Rigid) + self.actor_list.append(radar_sen) + return radar_sen + + def collision(self, vehicle): + collision_sensor = self.blueprint_library.find("sensor.other.collision") + with open('src/data/collision.csv', 'a') as csvfile: + ww = np.array(["time stamp", "situation"]) + ww = np.reshape(ww, (1, 2)) + np.savetxt(csvfile, ww, delimiter=',', fmt=['%s', '%s'], comments='') + collision_location = carla.Location(0, 0, 0) + collision_rotation = carla.Rotation(0, 0, 0) + collision_transform = carla.Transform(collision_location, collision_rotation) + collision_sen = self.world.spawn_actor(collision_sensor, collision_transform, attach_to=vehicle, + attachment_type=carla.AttachmentType.Rigid) + self.actor_list.append(collision_sen) + return collision_sen + + def rgb_cam(self, vehicle): rgb_camera = self.blueprint_library.find("sensor.camera.rgb") rgb_camera.set_attribute("image_size_x", f"{self.IM_WIDTH}") rgb_camera.set_attribute("image_size_y", f"{self.IM_HEIGHT}") @@ -59,40 +113,43 @@ def rgb_cam(self): spawn_point_rgb = carla.Transform(carla.Location(x=2.5, y=0, z=0.9), carla.Rotation(pitch=-5, roll=0, yaw=0)) - self.rgb_cam_sensor = self.world.spawn_actor(rgb_camera, spawn_point_rgb, attach_to=self.vehicle) + self.rgb_cam_sensor = self.world.spawn_actor(rgb_camera, spawn_point_rgb, attach_to=vehicle) self.actor_list.append(self.rgb_cam_sensor) return self.rgb_cam_sensor - def gnss(self): + def gnss(self, vehicle): gnss_sensor = self.blueprint_library.find("sensor.other.gnss") gnss_sensor.set_attribute("sensor_tick", str(0.0)) gnss_location = carla.Location(0, 0, 0) gnss_rotation = carla.Rotation(0, 0, 0) gnss_transform = carla.Transform(gnss_location, gnss_rotation) - ego_gnss = self.world.spawn_actor(gnss_sensor, gnss_transform, attach_to=self.vehicle, + ego_gnss = self.world.spawn_actor(gnss_sensor, gnss_transform, attach_to=vehicle, attachment_type=carla.AttachmentType.Rigid) self.actor_list.append(ego_gnss) return ego_gnss - def imu(self): + def imu(self, vehicle): imu_sensor = self.blueprint_library.find("sensor.other.imu") imu_location = carla.Location(0, 0, 0) imu_rotation = carla.Rotation(0, 0, 0) imu_transform = carla.Transform(imu_location, imu_rotation) - ego_imu = self.world.spawn_actor(imu_sensor, imu_transform, attach_to=self.vehicle, + ego_imu = self.world.spawn_actor(imu_sensor, imu_transform, attach_to=vehicle, attachment_type=carla.AttachmentType.Rigid) self.actor_list.append(ego_imu) return ego_imu + # callbacks + def rgb_cam_callback(self, image): - image.save_to_disk("data/rgb_cam/%06d.jpg" % image.frame) - 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() + image.save_to_disk("src/data/rgb_cam/%06d.jpg" % image.frame) + # 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() + # cv2.imshow("rgb camera", rgb_image) # FixMe: Replace with pygame visualization # cv2.waitKey(1) @@ -118,3 +175,25 @@ 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 lidar_callback(self, point_cloud): + data = np.copy(np.frombuffer(point_cloud.raw_data, dtype=np.dtype('f4'))) + # print("#########", len(point_cloud.raw_data), "#########") + data = np.reshape(data, (int(data.shape[0] / 3), 3)) + + with open('src/data/lidar.csv', 'a') as csvfile: + np.savetxt(csvfile, data, delimiter=',', fmt=['%f', '%f', '%f'], comments='') + + def collision_callback(self, event): + ar = np.array([str(event.timestamp), "collision detected!"]) + ar = np.reshape(ar, (1, 2)) + with open('src/data/collision.csv', 'a') as csvfile: + np.savetxt(csvfile, ar, delimiter=',', fmt=['%s', '%s'], comments='') + + def radar_callback(self, data): + + mydata = np.frombuffer(data.raw_data, dtype=np.dtype('f4')) + mydata = np.reshape(mydata, (int(mydata.shape[0] / 4), 4)) + with open('src/data/radar.csv', 'a') as csvfile: + np.savetxt(csvfile, mydata, delimiter=',', fmt=['%f', '%f', '%f', '%f'], comments='') + diff --git a/src/utils/carla_utils.py b/src/utils/carla_utils.py index 6297424..e5683b9 100644 --- a/src/utils/carla_utils.py +++ b/src/utils/carla_utils.py @@ -1,5 +1,7 @@ +import sys import threading from typing import Tuple, Any, Dict, Union +sys.path.append(r"D:\CARLA_0.9.8_2\WindowsNoEditor\PythonAPI\carla\dist\carla-0.9.8-py3.7-win-amd64.egg") import carla @@ -27,12 +29,14 @@ 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 = "Town05" + # road_id_list: list = [17,1000, 1,2000, 1000, 5,2000, 1000, 7,2000,1000, 17,2000] + # filtered_point_index_list: list = [-4,3, 0,1, 9, -1, 1,3, -1,1,8, 0,1] + road_id_list: list = [1,1,2000, 1000, 5,5,2000, 1000, 7,7,2000,1000, 17,17,2000,1000] + filtered_point_index_list: list = [ 0,28,1, 5, -1,-29, 1,4, -1,-29,1,8, 0,28,1,5] elif self.trajectory_index == 1: - carla_map = "Town10HD_Opt" - road_id_list: list = [1, 2000, 1, 2, 1000, 21, 20, 20, 1000, 15, 22, 6, 2000] + carla_map = "Town05" + road_id_list: list = [1, 1, 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: raise NotImplementedError('A trajectory with index {} has not been implemented.' @@ -41,7 +45,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: diff --git a/src/utils/controller.py b/src/utils/controller.py index e312b7d..28716b5 100644 --- a/src/utils/controller.py +++ b/src/utils/controller.py @@ -2,8 +2,9 @@ # # This work is licensed under the terms of the MIT license. # For a copy, see . - +import sys """ This module contains PID controllers to perform lateral and longitudinal control. """ +sys.path.append(r"D:\CARLA_0.9.8_2\WindowsNoEditor\PythonAPI\carla\dist\carla-0.9.8-py3.7-win-amd64.egg") from collections import deque import math @@ -170,7 +171,7 @@ def _pid_control(self, target_speed, current_speed): return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) - def change_parameters(self, K_P, K_I, K_D, dt): + def change_parameters(self, K_P, K_I, K_D, dt=0.03): """Changes the PID parameters""" self._k_p = K_P self._k_i = K_I @@ -261,7 +262,7 @@ def _pid_control(self, waypoint, vehicle_transform): return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) - def change_parameters(self, K_P, K_I, K_D, dt): + def change_parameters(self, K_P, K_I, K_D, dt=0.03): """Changes the PID parameters""" self._k_p = K_P self._k_i = K_I