A2.aghaei.401613003

src/__main__.py

@@ -1,30 +1,43 @@
 import time
-
+import sys
 import carla
 
 from src.simulator_handler import SimulatorHandler
 from utils.vehicle_command import VehicleCommand
+from path_following_handler import PathFollowingHandler
+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="Town10HD_Opt")
     simulator_handler.set_weather(weather=carla.WeatherParameters.ClearNoon)
 
-    # potential weather choices are [ClearNoon, ClearSunset, CloudyNoon, CloudySunset,
-    # WetNoon, WetSunset, MidRainyNoon, MidRainSunset, HardRainNoon, HardRainSunset,
-    # SoftRainNoon, SoftRainSunset]
-
     # 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()
+    colision_sensor = simulator_handler.colision()
 
     # 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_sensor: simulator_handler.lidar_callback(lidar_sensor))
+    radar_sensor.listen(lambda radar_sensor: simulator_handler.radar_callback(radar_sensor))
+    colision_sensor.listen(lambda colision_sensor: simulator_handler.colision_callback(colision_sensor))
+
+
+    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

@@ -3,14 +3,14 @@
 import sys
 from abc import ABC
 from typing import Any, Union, Dict, List
-
+sys.path.append("C:\Users\Sorosoh\Downloads\Compressed\WindowsNoEditor\PythonAPI\carla\dist\carla-0.9.10-py3.7-win-amd64.egg")
 import carla
 from carla import World
-
 from src.utils.carla_utils import draw_waypoints, filter_waypoints, TrajectoryToFollow, InfiniteLoopThread
 from src.utils.controller import VehiclePIDController
 
 
+
 class PathFollowingHandler(InfiniteLoopThread, ABC):
     def __init__(self, client: carla.Client, debug_mode: bool = False,
                  trajectory_index: int = 0) -> None:
@@ -34,12 +34,12 @@ def __init__(self, client: carla.Client, debug_mode: bool = False,
             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_D': 0.08,
              '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.08}  # control speed
         self.vehicle_to_target_distance_threshold: float = 2.5
 
         self.desired_speed: int = 20  # meter per second
@@ -117,6 +117,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=20)
+            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_
@@ -128,9 +139,7 @@ def __step__(self):
             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.infinite_trajectory_tracking(self.ego_vehicle, self.ego_pid_controller)
         else:
             sys.exit(1)
 

src/simulator_handler.py

@@ -1,12 +1,17 @@
 import os
 
+import math
 import carla
 import cv2
 import numpy as np
 import pandas as pd
 from matplotlib import pyplot as plt
+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
 
 
+# First I build structurs and then add details....
 class SimulatorHandler:
     def __init__(self, town_name):
         self.spawn_point = None
@@ -20,32 +25,38 @@ def __init__(self, town_name):
             os.makedirs(self.save_dir)
         if not os.path.exists(os.path.join(self.save_dir, "rgb_cam")):
             os.makedirs(os.path.join(self.save_dir, "rgb_cam"))
-
+        # connect to carla....
         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)
-
+            # control actors...
             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
+            self.IM_WIDTH = 1280
+            self.IM_HEIGHT = 720
             print("Successfully connected to CARLA client")
         except Exception as error:
             raise Exception(f"Error while initializing the simulator: {error}")
-
+        # for other sensors....
         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)
+    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)
@@ -54,10 +65,10 @@ def rgb_cam(self):
         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}")
-        rgb_camera.set_attribute("fov", "110")
+        rgb_camera.set_attribute("fov", "110") # in deg
         rgb_camera.set_attribute('sensor_tick', '0.0')
         spawn_point_rgb = carla.Transform(carla.Location(x=2.5, y=0, z=0.9),
-                                          carla.Rotation(pitch=-5, roll=0, yaw=0))
+                                          carla.Rotation(pitch=-5, roll=0, yaw=0)) # relative to the vehicle location
 
         self.rgb_cam_sensor = self.world.spawn_actor(rgb_camera, spawn_point_rgb, attach_to=self.vehicle)
         self.actor_list.append(self.rgb_cam_sensor)
@@ -73,8 +84,7 @@ def gnss(self):
                                           attachment_type=carla.AttachmentType.Rigid)
         self.actor_list.append(ego_gnss)
         return ego_gnss
-
-    def imu(self):
+def imu(self):
         imu_sensor = self.blueprint_library.find("sensor.other.imu")
         imu_location = carla.Location(0, 0, 0)
         imu_rotation = carla.Rotation(0, 0, 0)
@@ -84,19 +94,16 @@ def imu(self):
         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)
         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)
-
-    def imu_callback(self, imu):  # accelerometer is m/s^2 and gyroscope data is rad/sec
+    def imu_callback(self, imu):
         imu_dict = {}
         imu_dict["timestamp"] = imu.timestamp
         imu_dict["accelerometer_x"] = imu.accelerometer.x
@@ -108,7 +115,7 @@ 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 gnss_callback(self, gnss):
         gnss_dict = {}
@@ -117,4 +124,5 @@ 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")
+