Assignment 2-Mahdi Rahimi

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()
 

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,23 +39,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.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,44 +107,59 @@ 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],
                                    filtered_points_index=self.waypoints_to_visualize['filtered_points_index'][0])
             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)
 

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)