def get_bearing_at_waypoint(self, waypoint_id):
previous_point = self._track_waypoints[
self._get_previous_different_waypoint_id(waypoint_id)]
next_point = self._track_waypoints[
self._get_next_different_waypoint_id(waypoint_id)]
mid_point = self._track_waypoints[waypoint_id]
before_bearing = geometry.get_bearing_between_points(
previous_point, mid_point)
after_bearing = geometry.get_bearing_between_points(
mid_point, next_point)
change_in_bearing = geometry.get_angle_in_proper_range(after_bearing -
before_bearing)
return geometry.get_angle_in_proper_range(before_bearing +
change_in_bearing / 2)
def right_button_pressed(self, chosen_point):
chose_backwards_point = False
if self._backwards_point:
backwards_distance = geometry.get_distance_between_points(
chosen_point, self._backwards_point)
primary_distance = geometry.get_distance_between_points(
chosen_point, self._chosen_point)
chose_backwards_point = backwards_distance < 0.5 * primary_distance and abs(
primary_distance -
BACKWARDS_DISTANCE) < 0.25 * BACKWARDS_DISTANCE
if chose_backwards_point:
self._backwards_point = chosen_point
self._chosen_bearing = geometry.get_bearing_between_points(
self._backwards_point, self._chosen_point)
else:
self._chosen_point, waypoint_id = self.current_track.get_adjusted_point_on_track(
chosen_point)
self._chosen_bearing = self.current_track.get_bearing_at_waypoint(
waypoint_id)
self._backwards_point = geometry.get_point_at_bearing(
self._chosen_point, self._chosen_bearing - 180,
BACKWARDS_DISTANCE)
self.guru_parent_redraw()
def get_track_bearing_at_point(self, point):
closest_waypoint = self._get_closest_waypoint_id(point)
(before_waypoint,
after_waypoint) = self.get_waypoint_ids_before_and_after(
point, closest_waypoint)
return geometry.get_bearing_between_points(
self._track_waypoints[before_waypoint],
self._track_waypoints[after_waypoint])
def get_bearing_and_distance_to_next_waypoint(self, waypoint_id: int):
this_point = self._drawing_points[waypoint_id].middle
next_point = self._drawing_points[self._get_next_waypoint_id(
waypoint_id)].middle
distance = geometry.get_distance_between_points(this_point, next_point)
bearing = geometry.get_bearing_between_points(this_point, next_point)
return bearing, distance
def get_adjusted_point_on_track(self,
chosen_point: Point,
margin: float = 0.0):
waypoint_id = self.get_closest_waypoint_id(chosen_point)
waypoint = self.get_waypoint(waypoint_id)
distance_from_waypoint = geometry.get_distance_between_points(
waypoint, chosen_point)
max_distance_from_centre = (self.get_width() +
VEHICLE_WIDTH) / 2 - margin
if distance_from_waypoint > max_distance_from_centre:
bearing_of_point = geometry.get_bearing_between_points(
waypoint, chosen_point)
adjusted_point = geometry.get_point_at_bearing(
waypoint, bearing_of_point, max_distance_from_centre)
else:
adjusted_point = chosen_point
return adjusted_point, waypoint_id
def set_true_bearing_and_slide_on_events(self):
previous_event = self.events[0]
self.events[0].slide = 0.0
self.events[0].true_bearing = self.events[0].heading
self.max_slide = 0.0
for e in self.events[1:]:
previous_location = (previous_event.x, previous_event.y)
current_location = (e.x, e.y)
if e.progress == previous_event.progress: # Handle new AWS bug duplicating log entry position
e.true_bearing = previous_event.true_bearing
elif e.progress - previous_event.progress < 0.05: # x and y rounding means slow progress is inaccurate
e.true_bearing = previous_event.true_bearing
else:
e.true_bearing = get_bearing_between_points(
previous_location, current_location)
e.slide = get_turn_between_directions(e.heading, e.true_bearing)
if e.step > SLIDE_SETTLING_PERIOD:
self.max_slide = max(self.max_slide, abs(e.slide))
previous_event = e
def get_projected_distance_on_track(self,
point: Point,
heading: float,
closest_waypoint_id: int,
path_width: float = 0.0):
heading = geometry.get_angle_in_proper_range(heading)
if path_width > 0.0:
side_point_1 = geometry.get_point_at_bearing(
point, heading + 90, path_width / 2)
side_point_2 = geometry.get_point_at_bearing(
point, heading - 90, path_width / 2)
return min(
self.get_projected_distance_on_track(point, heading,
closest_waypoint_id),
self.get_projected_distance_on_track(side_point_1, heading,
closest_waypoint_id),
self.get_projected_distance_on_track(side_point_2, heading,
closest_waypoint_id))
(before_waypoint_id,
after_waypoint_id) = self.get_waypoint_ids_before_and_after(
point, closest_waypoint_id, True)
previous_left = self._drawing_points[before_waypoint_id].left_safe
previous_right = self._drawing_points[before_waypoint_id].right_safe
for w in self._drawing_points[
after_waypoint_id:] + self._drawing_points[:after_waypoint_id]:
direction_to_left_target = geometry.get_bearing_between_points(
point, w.left_safe)
direction_to_right_target = geometry.get_bearing_between_points(
point, w.right_safe)
relative_direction_to_left_target = geometry.get_turn_between_directions(
heading, direction_to_left_target)
relative_direction_to_right_target = geometry.get_turn_between_directions(
heading, direction_to_right_target)
if relative_direction_to_left_target >= 0 and relative_direction_to_right_target <= 0:
previous_left = w.left_safe
previous_right = w.right_safe
else:
point2 = geometry.get_point_at_bearing(
point, heading, 1) # Just some random distance (1m)
if w.left_safe == previous_left:
off_track_left = previous_left
else:
off_track_left = geometry.get_intersection_of_two_lines(
point, point2, w.left_safe, previous_left)
if w.right_safe == previous_right:
off_track_right = previous_right
else:
off_track_right = geometry.get_intersection_of_two_lines(
point, point2, w.right_safe, previous_right)
left_bearing = geometry.get_bearing_between_points(
point, off_track_left)
right_bearing = geometry.get_bearing_between_points(
point, off_track_right)
distances = []
if abs(
geometry.get_turn_between_directions(
left_bearing, heading)) < 1:
if geometry.is_point_between(off_track_left, w.left_safe,
previous_left):
distances += [
geometry.get_distance_between_points(
point, off_track_left)
]
if abs(
geometry.get_turn_between_directions(
right_bearing, heading)) < 1:
if geometry.is_point_between(off_track_right, w.right_safe,
previous_right):
distances += [
geometry.get_distance_between_points(
point, off_track_right)
]
if len(distances) > 0:
return max(distances)
else:
return 0.0