[Bug]: Pure Pursuit Controller Indexing Bug

[vinzenzm]

Current Behavior

In pure_pursuit_controller.py the method __get_target_point_index calculates the next wapoint in the trajectory that is to be followed.
For a static trajectory this is fine, if trajectory indexing changes however this calculation is not correct.

The car then does not follow the points we expect.

Expected Behavior

The car should be able to follow any trajectory correctly.

Steps to Reproduce

E.g. send a localized trajectory to the controller.

Definition of Done

The underlying issue needs to be fixed.

Testability

Different trajectory scenarios need to be tested.

Dependencies

No response

[vinzenzm]

This is not as straight forward as one might expect.
If we remove this index and always follow e.g. the closest point unpredictable behavior is to be expected.

I already tested to ignore trajectory indices completely and implemented a simple __is_ahead method based on current heading:

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

This works as long as the car points in the generally correct orientation, if a crash causes us to flip more than 90° however we start following the trajectory in reverse.

We need to find a way to use the "forwardness" of the trajectory (higher index tells us the waypoint should be visited later) while not beeing bound to indices of a mutable trajectory.