Skip to content

Assignment #2 #18

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
hasannoori wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Assignment #2 #18

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
104 changes: 89 additions & 15 deletions src/__main__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,30 +1,104 @@

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_0.9.8_2\WindowsNoEditor\PythonAPI\carla\dist\carla-0.9.8-py3.7-win-amd64.egg")

sys.path.append(r"D:\CARLA_Code\integrate two session\src")
from utils.carla_utils import draw_waypoints, filter_waypoints, TrajectoryToFollow, InfiniteLoopThread

import time

import carla

from src.simulator_handler import SimulatorHandler
from utils.vehicle_command import VehicleCommand

if __name__ == "__main__":
simulator_handler = SimulatorHandler(town_name="Town04")
simulator_handler.spawn_vehicle(spawn_index=13)
simulator_handler.set_weather(weather=carla.WeatherParameters.ClearNoon)
sys.path.append(r"D:\CARLA_Code\integrate two session\src")
sys.path.append(r"D:\CARLA_Code\integrate two session\utils")

from simulator_handler import SimulatorHandler
from path_following_handler import PathFollowingHandler
from vehicle_command import VehicleCommand





if __name__ == '__main__':
client = carla.Client("localhost", 2000)
client.set_timeout(8.0)

town_name="Town05"
# spawn_index = 2

try:
print("Trying to communicate with the client...")
world = client.get_world()
if os.path.basename(world.get_map().name) != town_name:
world: carla.World = client.load_world(town_name)

blueprint_library = world.get_blueprint_library()
actor_list = []
print("Successfully connected to CARLA client")
except Exception as error:
raise Exception(f"Error while initializing the simulator: {error}")

simulator_handler = SimulatorHandler(client=client,actor_list=actor_list)




world.set_weather(carla.WeatherParameters.ClearNoon)


path_following_handler = PathFollowingHandler(client=client, debug_mode=False)

vehicle_blueprint = blueprint_library.filter("model3")[0] # choosing the car
# spawn_point = world.get_map().get_spawn_points()[spawn_index]
# vehicle = world.spawn_actor(vehicle_blueprint, spawn_point)

ego_spawn_point = path_following_handler.ego_spawn_point


# potential weather choices are [ClearNoon, ClearSunset, CloudyNoon, CloudySunset,
# WetNoon, WetSunset, MidRainyNoon, MidRainSunset, HardRainNoon, HardRainSunset,
# SoftRainNoon, SoftRainSunset]
filtered_waypoints = filter_waypoints(path_following_handler.waypoints, road_id=ego_spawn_point["road_id"])
spawn_point = filtered_waypoints[ego_spawn_point["filtered_points_index"]].transform
spawn_point.location.z += 2
vehicle = client.get_world().spawn_actor(vehicle_blueprint, spawn_point)
actor_list.append(vehicle)

# add sensors
rgb_cam = simulator_handler.rgb_cam()
gnss_sensor = simulator_handler.gnss()
imu_sensor = simulator_handler.imu()

rgb_cam = simulator_handler.rgb_cam(vehicle)
gnss_sensor = simulator_handler.gnss(vehicle)
imu_sensor = simulator_handler.imu(vehicle)
lidar = simulator_handler.lidar(vehicle)
radar = simulator_handler.radar(vehicle)
collision = simulator_handler.collision(vehicle)

# 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.listen(lambda data: simulator_handler.lidar_callback(data))
radar.listen(lambda data: simulator_handler.radar_callback(data))
collision.listen(lambda event: simulator_handler.collision_callback(event))




if path_following_handler.debug_mode:
path_following_handler.start()
else:
ego_pid_controller = path_following_handler.pid_controller(vehicle,
path_following_handler.pid_values_lateral,
path_following_handler.pid_values_longitudinal)

path_following_handler.vehicle_and_controller_inputs(vehicle, ego_pid_controller)
path_following_handler.start()
130 changes: 85 additions & 45 deletions src/path_following_handler.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,12 +3,19 @@
import sys
from abc import ABC
from typing import Any, Union, Dict, List
from datetime import datetime
import numpy as np

sys.path.append(r"D:\CARLA_0.9.8_2\WindowsNoEditor\PythonAPI\carla\dist\carla-0.9.8-py3.7-win-amd64.egg")
sys.path.append(r"D:\CARLA_Code\integrate two session\src")
sys.path.append(r"D:\CARLA_Code\integrate two session\utils")


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):
Expand All @@ -31,28 +38,43 @@ def __init__(self, client: carla.Client, debug_mode: bool = False,
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.waypoints_to_visualize = {'road_id': [1], '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_P': 2,
'K_D': 0,
'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
self.vehicle_to_target_distance_threshold: float = 2.5

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

# self.i = 0
# self.intersectionCounter =0
# for x in range(4):
# with open("intersection"+str(x)+".csv", 'a') as csvfile:
# ww = np.array(["timeStamp","P,I,D", "Yaw"])
# ww = np.reshape(ww, (1, 3))
# np.savetxt(csvfile, ww, delimiter=',', fmt=['%s', '%s', '%s'], comments='')



def __follow_target_waypoints__(self, vehicle: Any, target_waypoint, ego_pid_controller_) -> None:
self.client.get_world().debug.draw_string(target_waypoint.transform.location, 'O',
draw_shadow=False,
color=carla.Color(r=255, g=0, b=0),
life_time=20,
persistent_lines=True)

# dict = {}
self.i = 0

while True:
self.i = self.i+1
vehicle_loc = vehicle.get_location()
vehicle_to_target_distance = math.sqrt((target_waypoint.transform.location.x - vehicle_loc.x) ** 2
+ (target_waypoint.transform.location.y - vehicle_loc.y) ** 2)
Expand All @@ -63,6 +85,15 @@ def __follow_target_waypoints__(self, vehicle: Any, target_waypoint, ego_pid_con
else:
control_signal = ego_pid_controller_.run_step(desired_speed, target_waypoint)
vehicle.apply_control(control_signal)
# print("***************", vehicle.get_transform().rotation.yaw)

# if self.i % 500 == 0:
# with open("intersection"+str(self.intersectionCounter%4)+".csv", 'a') as csvfile:
# yaw = vehicle.get_transform().rotation.yaw
# PID = str(self.pid_values_lateral['K_P'])+" ,"+str(self.pid_values_lateral['K_I'])+" ,"+str(self.pid_values_lateral['K_D'])
# data = np.array([str(datetime.now()),PID, str(yaw)])
# data = np.reshape(data, (1, 3))
# np.savetxt(csvfile, data, delimiter=',', fmt=['%s', '%s', '%s'], comments='')

def visualize_road_id(self, road_id: int, filtered_points_index: int, life_time: int = 5) -> None:
# For debugging purposes.
Expand Down Expand Up @@ -93,29 +124,36 @@ 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
# self.intersectionCounter +=1
# if self.intersectionCounter%4 == 0:
# self.pid_values_lateral['K_D']+=0.05
# ego_pid_controller_._lat_controller.change_parameters(**self.pid_values_lateral)
# print("************K_D=",self.pid_values_lateral['K_D'],"**************")
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_
Expand All @@ -139,19 +177,21 @@ def terminate(self):
self.join()


if __name__ == '__main__':
client_ = carla.Client("localhost", 2000)
client_.set_timeout(8.0)
path_following_handler = PathFollowingHandler(client=client_, debug_mode=False)
ego_spawn_point = path_following_handler.ego_spawn_point
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()
# if __name__ == '__main__':
# client_ = carla.Client("localhost", 2000)
# client_.set_timeout(8.0)
# path_following_handler = PathFollowingHandler(client=client_, debug_mode=False)
# ego_spawn_point = path_following_handler.ego_spawn_point
# 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()
Loading