Skip to content

A2-Zareshahi-Khademi-Azizi Asl #19

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
hamidrezaz1376 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

A2-Zareshahi-Khademi-Azizi Asl #19

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 25 additions & 8 deletions src/__main__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,23 @@

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]

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()
Expand All @@ -23,8 +30,18 @@
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()
41 changes: 29 additions & 12 deletions src/path_following_handler.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
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:
Expand All @@ -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)
Expand All @@ -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.08,
'K_I': 0.05}
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.07}
self.vehicle_to_target_distance_threshold: float = 2.5

self.desired_speed: int = 20 # meter per second
self.desired_speed: int = 20
self.reached_destination: bool = False
self.previous_waypoint: Union[carla.Waypoint, None] = None

Expand All @@ -65,7 +67,7 @@ def __follow_target_waypoints__(self, vehicle: Any, target_waypoint, ego_pid_con
vehicle.apply_control(control_signal)

def visualize_road_id(self, road_id: int, filtered_points_index: int, life_time: int = 5) -> None:
# For debugging purposes.

filtered_waypoints = filter_waypoints(self.waypoints, road_id=road_id)
draw_waypoints(self.world, self.waypoints, road_id=road_id, life_time=life_time)
target_waypoint = filtered_waypoints[filtered_points_index]
Expand Down Expand Up @@ -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], \
Expand All @@ -102,10 +105,10 @@ def follow_trajectory(self, vehicle: Any, ego_pid_controller_: VehiclePIDControl
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
if current_road_id == 1000:
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
elif current_road_id == 2000:
target_waypoints: List[carla.Waypoint] = self.previous_waypoint.next_until_lane_end(float(
current_filtered_point_index))
for target_waypoint in target_waypoints:
Expand All @@ -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}
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_
Expand All @@ -128,9 +145,9 @@ 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)

Expand Down
50 changes: 32 additions & 18 deletions src/simulator_handler.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -23,29 +26,38 @@ 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({})
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)


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)
Expand Down Expand Up @@ -90,11 +102,7 @@ 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 = {}
Expand All @@ -108,7 +116,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 = {}
Expand All @@ -117,4 +125,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()