diff --git a/src/__main__.py b/src/__main__.py index fdb8552..9498181 100644 --- a/src/__main__.py +++ b/src/__main__.py @@ -6,8 +6,8 @@ from utils.vehicle_command import VehicleCommand if __name__ == "__main__": - simulator_handler = SimulatorHandler(town_name="Town04") - simulator_handler.spawn_vehicle(spawn_index=13) + simulator_handler = SimulatorHandler(town_name="Town03") + simulator_handler.spawn_vehicle(spawn_index=30) simulator_handler.set_weather(weather=carla.WeatherParameters.ClearNoon) # potential weather choices are [ClearNoon, ClearSunset, CloudyNoon, CloudySunset, @@ -15,16 +15,23 @@ # SoftRainNoon, SoftRainSunset] # add sensors + LIDAR_sensor = simulator_handler.LIDAR() + radar_sensor = simulator_handler.radar() + collision_sensor = simulator_handler.collision() rgb_cam = simulator_handler.rgb_cam() gnss_sensor = simulator_handler.gnss() imu_sensor = simulator_handler.imu() # listen to sensor data + 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)) 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) + + time.sleep(1000.0) diff --git a/src/adverse_weather_classification/LO-AC.png b/src/adverse_weather_classification/LO-AC.png new file mode 100644 index 0000000..2254992 Binary files /dev/null and b/src/adverse_weather_classification/LO-AC.png differ diff --git a/src/adverse_weather_classification/train.py b/src/adverse_weather_classification/train.py index 8822b6b..c13226f 100644 --- a/src/adverse_weather_classification/train.py +++ b/src/adverse_weather_classification/train.py @@ -1,6 +1,5 @@ import os from typing import Tuple - import matplotlib import numpy as np import tensorflow as tf @@ -10,7 +9,6 @@ from mock import Mock import matplotlib.pyplot as plt - 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: diff --git a/src/adverse_weather_classification/weather_classification.py b/src/adverse_weather_classification/weather_classification.py index 777ddfe..1c8a2d5 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 = ['ClearNoon', 'ClearSunset','fog','HardRainSunset'] 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:\Set\TestFinal" + model_path_ = r"D:\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..c6ba99d 100644 --- a/src/path_following_handler.py +++ b/src/path_following_handler.py @@ -3,7 +3,8 @@ import sys from abc import ABC from typing import Any, Union, Dict, List - +from datetime import datetime +import numpy as np import carla from carla import World @@ -26,23 +27,24 @@ 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) 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, - 'K_D': 0.07, - 'K_I': 0.05} # control speed + {'K_P': 1.5, + 'K_D': 0.08, + 'K_I': 0.07} # control speed self.vehicle_to_target_distance_threshold: float = 2.5 - self.desired_speed: int = 20 # meter per second + self.desired_speed: int = 30 # meter per second self.reached_destination: bool = False self.previous_waypoint: Union[carla.Waypoint, None] = None @@ -117,6 +119,17 @@ 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} + 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_ @@ -130,7 +143,7 @@ def __step__(self): if not self.reached_destination: self.follow_trajectory(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..7908c24 100644 --- a/src/simulator_handler.py +++ b/src/simulator_handler.py @@ -1,13 +1,17 @@ import os - +import sys +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 import carla import cv2 import numpy as np import pandas as pd from matplotlib import pyplot as plt +import datetime +import glob - -class SimulatorHandler: +class SimulatorHandler(): def __init__(self, town_name): self.spawn_point = None self.vehicle = None @@ -23,9 +27,9 @@ 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) @@ -40,12 +44,27 @@ 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({}) + self.waypoints: list = self.client.get_world().get_map().generate_waypoints(distance=1.0) + + for actor in self.world.get_actors().filter('*vehicle*'): + actor.destroy() + for actor in self.world.get_actors().filter('*sensor*'): + actor.destroy() + + 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) @@ -83,6 +102,7 @@ def imu(self): attachment_type=carla.AttachmentType.Rigid) self.actor_list.append(ego_imu) return ego_imu + def rgb_cam_callback(self, image): image.save_to_disk("data/rgb_cam/%06d.jpg" % image.frame) @@ -95,7 +115,41 @@ def rgb_cam_callback(self, image): plt.show() # cv2.imshow("rgb camera", rgb_image) # FixMe: Replace with pygame visualization # cv2.waitKey(1) - + 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 imu_callback(self, imu): # accelerometer is m/s^2 and gyroscope data is rad/sec imu_dict = {} imu_dict["timestamp"] = imu.timestamp @@ -108,8 +162,35 @@ 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 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) + def gnss_callback(self, gnss): gnss_dict = {} gnss_dict["timestamp"] = gnss.timestamp @@ -117,4 +198,12 @@ 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() + +