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

@@ -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))