Skip to content

Update path_following_handler.py #2

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

Update path_following_handler.py #2

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
41 changes: 31 additions & 10 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,13 @@
import sys
from abc import ABC
from typing import Any, Union, Dict, List

#Pooriya Sanaie(401624033)-Fateme Aghabarari(401622503)-Seyed Javad Hosseini
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 src.simulator_handler import SimulatorHandler


class PathFollowingHandler(InfiniteLoopThread, ABC):
Expand All @@ -26,20 +27,24 @@ 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)
self.debug_mode = debug_mode
if self.debug_mode:
self.waypoints_to_visualize = {'road_id': [4], 'filtered_points_index': [0]}
# (The PID controller may be tuned using a variety of techniques, including Twiddle, WAF-Tune, and Ziegler-Nichols.
# The WAF-Tune approach produced the best results in our tests, albeit the outcomes were not particularly satisfactory.)
# (Method WAF-Tune gave us the lowest MSE)
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_P': 0.35,
'K_D': 6.5,
'K_I': 0.0005} # 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': 2,
'K_D': 0.8,
'K_I': 0.1} # control speed
self.vehicle_to_target_distance_threshold: float = 2.5

self.desired_speed: int = 20 # meter per second
Expand Down Expand Up @@ -93,6 +98,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 @@ -117,6 +123,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_
Expand All @@ -128,9 +148,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)

Expand Down