diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 0000000..13566b8
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
diff --git a/.idea/CARLA_tutorial.iml b/.idea/CARLA_tutorial.iml
new file mode 100644
index 0000000..8b8c395
--- /dev/null
+++ b/.idea/CARLA_tutorial.iml
@@ -0,0 +1,12 @@
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml
new file mode 100644
index 0000000..105ce2d
--- /dev/null
+++ b/.idea/inspectionProfiles/profiles_settings.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000..d56657a
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,4 @@
+
+
+
+
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
new file mode 100644
index 0000000..f4454f1
--- /dev/null
+++ b/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000..35eb1dd
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/__main__.py b/src/__main__.py
index fdb8552..dbd48de 100644
--- a/src/__main__.py
+++ b/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()
+
diff --git a/src/path_following_handler.py b/src/path_following_handler.py
index b7a75c6..a5d6f63 100644
--- a/src/path_following_handler.py
+++ b/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)
diff --git a/src/simulator_handler.py b/src/simulator_handler.py
index ba3b914..2240096 100644
--- a/src/simulator_handler.py
+++ b/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,7 +25,7 @@ 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)
@@ -28,24 +33,30 @@ def __init__(self, town_name):
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")
+