def signed_lateral_error(self, p):
"""Returns the signed lateral distance from the given point to the
line formed by the waypoint position and the waypoints heading.
Negative signals right of line and Positive left of line
"""
return signed_dist_to_line(p, self.pos, self.heading.direction_vector())
def calulate_heading_lateral_error(
vehicle, trajectory, initial_look_ahead_distant, speed_reduction_activation
):
heading_error = min_angles_difference_signed(
(vehicle.heading % (2 * math.pi)), trajectory[2][0]
)
# Number of look ahead points to calculate the
# look ahead error.
# TODO: Find the number of points using the
# distance between trajectory points.
look_ahead_points = initial_look_ahead_distant
# If we are on the curvy portion of the road
# we need to decrease the values for look ahead calculation
# The value 30 for road curviness is obtained empirically
# for bullet model.
if (
abs(TrajectoryTrackingController.curvature_calculation(trajectory, 4)) < 30
and speed_reduction_activation
):
initial_look_ahead_distant = 1
look_ahead_points = 1
path_vector = radians_to_vec(
trajectory[2][min([look_ahead_points, len(trajectory[2]) - 1])]
)
vehicle_look_ahead_pt = [
vehicle.position[0]
- initial_look_ahead_distant * math.sin(vehicle.heading),
vehicle.position[1]
+ initial_look_ahead_distant * math.cos(vehicle.heading),
]
lateral_error = signed_dist_to_line(
vehicle_look_ahead_pt,
[
trajectory[0][min([look_ahead_points, len(trajectory[0]) - 1])],
trajectory[1][min([look_ahead_points, len(trajectory[1]) - 1])],
],
path_vector,
)
return (heading_error, lateral_error)