From f700710c4f5af344db3377b80d58933a02a65841 Mon Sep 17 00:00:00 2001 From: vinzenzm Date: Wed, 11 Dec 2024 17:49:45 +0100 Subject: [PATCH 1/2] Trying pure pursuit only. This commit was created in order to run full leaderboard test in the git action. Car is only steering with pure pursuit controller. I also added a fix for a known indexing Bug in the pure pursuit contoller. The fix is commented out, because it introduces a more severe Bug. --- code/control/src/pure_pursuit_controller.py | 26 +++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/code/control/src/pure_pursuit_controller.py b/code/control/src/pure_pursuit_controller.py index fbf6526b..9d443bec 100755 --- a/code/control/src/pure_pursuit_controller.py +++ b/code/control/src/pure_pursuit_controller.py @@ -10,7 +10,7 @@ from std_msgs.msg import Float32 from acting.msg import Debug import numpy as np - +from typing import Tuple from acting.helper_functions import vector_angle, points_to_vector # Tuneable Values for PurePursuit-Algorithm @@ -169,16 +169,38 @@ def __get_target_point_index(self, ld: float) -> int: min_dist_idx = -1 # might be more elegant to only look at points # _ahead_ of the closest point on the trajectory - for i in range(self.__tp_idx, len(self.__path.poses)): + for i in range(self.__tp_idx, len(self.__path.poses)): # self.__tp_idx, pose: PoseStamped = self.__path.poses[i] + # if not self.__is_ahead((pose.pose.position.x, pose.pose.position.y)): + # continue dist = self.__dist_to(pose.pose.position) dist2ld = dist - ld # can be optimized 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): + rotation_matrix = np.array( + [ + [np.cos(angle_rad), -np.sin(angle_rad)], + [np.sin(angle_rad), np.cos(angle_rad)], + ] + ) + + return rotation_matrix @ np.array(vector) + + def __is_ahead(self, pos: Tuple[float, float]): + x, y = pos + c_x, c_y = self.__position + to_car = np.array([x - c_x, y - c_y]) + heading = self.rotate_vector_2d(np.array([1.0, 0.0]), self.__heading) + + return np.dot(to_car, heading) > 1 + def __dist_to(self, pos: Point) -> float: """ Distance between current position and target position (only (x,y)) From a031a757089081445106c9cf159f8f37db09fdc0 Mon Sep 17 00:00:00 2001 From: vinzenzm Date: Fri, 13 Dec 2024 19:15:06 +0100 Subject: [PATCH 2/2] 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