From a031a757089081445106c9cf159f8f37db09fdc0 Mon Sep 17 00:00:00 2001 From: vinzenzm Date: Fri, 13 Dec 2024 19:15:06 +0100 Subject: [PATCH] Implemented the pure pursuit only, which wasnt pushed in last commit --- code/control/src/pure_pursuit_controller.py | 2 -- code/control/src/vehicle_controller.py | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/code/control/src/pure_pursuit_controller.py b/code/control/src/pure_pursuit_controller.py index 9d443bec..599271a1 100755 --- a/code/control/src/pure_pursuit_controller.py +++ b/code/control/src/pure_pursuit_controller.py @@ -179,8 +179,6 @@ def __get_target_point_index(self, ld: float) -> int: if min_dist > dist2ld > 0: min_dist = dist2ld min_dist_idx = i - - self.loginfo(str(min_dist_idx)) return min_dist_idx def rotate_vector_2d(self, vector, angle_rad): diff --git a/code/control/src/vehicle_controller.py b/code/control/src/vehicle_controller.py index 90aa0f68..2354c7d2 100755 --- a/code/control/src/vehicle_controller.py +++ b/code/control/src/vehicle_controller.py @@ -145,7 +145,7 @@ def loop(timer_event=None) -> None: return # Velocities over 5 m/s = use Stanley, else use PurePuresuit - if self.__velocity > 5: + if False: # self.__velocity > 5: steer = self._s_steer else: # while doing the unstuck routine we don't want to steer