From 5c96bb393be3cb464900c5b0b53c0520cc51fc8a Mon Sep 17 00:00:00 2001
From: PooriyaSanaie <88898162+ELECEngineer@users.noreply.github.com>
Date: Sat, 10 Jun 2023 22:41:27 +0330
Subject: [PATCH] Update path_following_handler.py

---
 src/path_following_handler.py | 41 ++++++++++++++++++++++++++---------
 1 file changed, 31 insertions(+), 10 deletions(-)

diff --git a/src/path_following_handler.py b/src/path_following_handler.py
index b7a75c6..f392fda 100644
--- a/src/path_following_handler.py
+++ b/src/path_following_handler.py
@@ -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):
@@ -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
@@ -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], \
@@ -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_
@@ -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)