diff --git a/src/__main__.py b/src/__main__.py index fdb8552..01695a4 100644 --- a/src/__main__.py +++ b/src/__main__.py @@ -1,30 +1,71 @@ import time import carla +import glob +import os +import sys from src.simulator_handler import SimulatorHandler from utils.vehicle_command import VehicleCommand +from utils.carla_utils import draw_waypoints, filter_waypoints, TrajectoryToFollow, InfiniteLoopThread +import warnings +warnings.filterwarnings("ignore") 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, # WetNoon, WetSunset, MidRainyNoon, MidRainSunset, HardRainNoon, HardRainSunset, # SoftRainNoon, SoftRainSunset] + + ego_spawn_point = path_following_handler.ego_spawn_point + 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) # 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() + rgb_cam = simulator_handler.rgb_cam(vehicle) + 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 + + LIDAR_sensor.listen(lambda LIDAR: simulator_handler.LIDAR_callback(LIDAR)) + 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)) + 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) + + 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() + + time.sleep(1000.0) diff --git a/src/simulator_handler.py b/src/simulator_handler.py index ba3b914..8af528f 100644 --- a/src/simulator_handler.py +++ b/src/simulator_handler.py @@ -1,11 +1,12 @@ import os - +import sys import carla import cv2 import numpy as np import pandas as pd from matplotlib import pyplot as plt - +import datetime +import glob class SimulatorHandler: def __init__(self, town_name): @@ -40,6 +41,14 @@ def __init__(self, town_name): self.imu_dataframe = pd.DataFrame({}) self.gnss_dataframe = pd.DataFrame({}) + 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() def spawn_vehicle(self, spawn_index: int = 90): self.vehicle_blueprint = self.blueprint_library.filter("model3")[0] # choosing the car @@ -83,6 +92,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 +105,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 @@ -109,7 +153,33 @@ def imu_callback(self, imu): # accelerometer is m/s^2 and gyroscope data is rad 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) - + 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