A2 mortezahosseini 401624827

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)
 
 
 

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