diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 0000000..26d3352
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,3 @@
+# Default ignored files
+/shelf/
+/workspace.xml
diff --git a/.idea/CARLA_tutorialA2mosayyebi-golabadi.iml b/.idea/CARLA_tutorialA2mosayyebi-golabadi.iml
new file mode 100644
index 0000000..8b8c395
--- /dev/null
+++ b/.idea/CARLA_tutorialA2mosayyebi-golabadi.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..e5ec441
--- /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..b83448e 100644
--- a/src/__main__.py
+++ b/src/__main__.py
@@ -4,27 +4,43 @@
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.set_weather(weather=carla.WeatherParameters.ClearNoon)
+ simulator_handler = SimulatorHandler(town_name="Town10HD_Opt")
- # potential weather choices are [ClearNoon, ClearSunset, CloudyNoon, CloudySunset,
- # WetNoon, WetSunset, MidRainyNoon, MidRainSunset, HardRainNoon, HardRainSunset,
- # SoftRainNoon, SoftRainSunset]
+ simulator_handler.set_weather(weather=carla.WeatherParameters.ClearNoon)
- # add sensors
+ # potential weather choices we have [ ClearSunset,CloudySunset,CloudyNoon,ClearNoon]
+ # WetNoon, WetSunset,HardRainNoon,HardRainSunset,MidRainyNoon,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"])
+ # sensors we have:
rgb_cam = simulator_handler.rgb_cam()
gnss_sensor = simulator_handler.gnss()
imu_sensor = simulator_handler.imu()
- # listen to sensor data
+ # 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)
-
-
-
+ # VehicleCommand(throttle=1.0).send_control(simulator_handler.vehicle)
+
+ if path_following_handler.debug_mode:
+ path_following_handler.start()
+ else:
+ # ego_vehicle = path_following_handler.spawn_ego_vehicles(road_id=ego_spawn_point["road_id"],
+ # filtered_points_index=ego_spawn_point["filtered_points_index"])
+ 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()
+
+ # time.sleep(20.0)
+ # simulator_handler.clearing()
diff --git a/src/path_following_handler.py b/src/path_following_handler.py
index b7a75c6..f894029 100644
--- a/src/path_following_handler.py
+++ b/src/path_following_handler.py
@@ -9,6 +9,7 @@
from src.utils.carla_utils import draw_waypoints, filter_waypoints, TrajectoryToFollow, InfiniteLoopThread
from src.utils.controller import VehiclePIDController
+# from src.simulator_handler import SimulatorHandler
class PathFollowingHandler(InfiniteLoopThread, ABC):
@@ -26,6 +27,7 @@ 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)
@@ -34,15 +36,15 @@ 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_I': 0.05} # control steering
+ 'K_D': 0.09,
+ 'K_I': 0.04} #steering control
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.09,
+ 'K_I': 0.08} #speed control
self.vehicle_to_target_distance_threshold: float = 2.5
- self.desired_speed: int = 20 # meter per second
+ self.desired_speed: int = 20 #meter per second
self.reached_destination: bool = False
self.previous_waypoint: Union[carla.Waypoint, None] = None
@@ -93,6 +95,7 @@ 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], \
@@ -117,6 +120,20 @@ 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} #get first way point
+ 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=30)
+ 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 +145,10 @@ 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.follow_trajectory(self.ego_vehicle, self.ego_pid_controller)
+ self.infinite_trajectory_tracking(self.ego_vehicle, self.ego_pid_controller)
+ # self.reached_destination = True
+ # print("Destination has been reached.")
else:
sys.exit(1)
diff --git a/src/simulator_handler.py b/src/simulator_handler.py
index ba3b914..a3fff29 100644
--- a/src/simulator_handler.py
+++ b/src/simulator_handler.py
@@ -5,9 +5,12 @@
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
-class SimulatorHandler:
+class SimulatorHandler():
def __init__(self, town_name):
self.spawn_point = None
self.vehicle = None
@@ -23,29 +26,42 @@ 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 = []
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 # Ideally a config file should be defined for such parameters
+ self.IM_HEIGHT = 720
print("Successfully connected to CARLA client")
except Exception as error:
raise Exception(f"Error while initializing the simulator: {error}")
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.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] # selecting 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 += 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)
@@ -90,11 +106,6 @@ def rgb_cam_callback(self, image):
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
imu_dict = {}
@@ -108,7 +119,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 +128,10 @@ 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')
+
+ def clearing(self):
+ for actor in self.world.get_actors().filter('*vehicle*'):
+ actor.destroy()
+ for actor in self.world.get_actors().filter('*sensor*'):
+ actor.destroy()