diff --git a/common/ferrostar/src/navigation_controller/mod.rs b/common/ferrostar/src/navigation_controller/mod.rs index cb1a8bec..f6508685 100644 --- a/common/ferrostar/src/navigation_controller/mod.rs +++ b/common/ferrostar/src/navigation_controller/mod.rs @@ -120,18 +120,31 @@ impl NavigationController { let mut remaining_steps = remaining_steps.clone(); remaining_steps.remove(0); + // we need to update the geometry index, since the step has changed + let (updated_current_step_geometry_index, updated_snapped_user_location) = + if let Some(current_route_step) = remaining_steps.first() { + // TODO: We could move this to the Route struct or NavigationController directly to only calculate it once. + let current_step_linestring = current_route_step.get_linestring(); + self.snap_user_to_line( + *snapped_user_location, + ¤t_step_linestring, + ) + } else { + (*current_step_geometry_index, *snapped_user_location) + }; + // Update remaining waypoints - let should_advance_waypoint = if let Some(waypoint) = - remaining_waypoints.first() - { - let current_location: Point = snapped_user_location.coordinates.into(); - let next_waypoint: Point = waypoint.coordinate.into(); - // TODO: This is just a hard-coded threshold for the time being. - // More sophisticated behavior will take some time and use cases, so punting on this for now. - Haversine::distance(current_location, next_waypoint) < 100.0 - } else { - false - }; + let should_advance_waypoint = + if let Some(waypoint) = remaining_waypoints.first() { + let current_location: Point = + updated_snapped_user_location.coordinates.into(); + let next_waypoint: Point = waypoint.coordinate.into(); + // TODO: This is just a hard-coded threshold for the time being. + // More sophisticated behavior will take some time and use cases, so punting on this for now. + Haversine::distance(current_location, next_waypoint) < 100.0 + } else { + false + }; let remaining_waypoints = if should_advance_waypoint { let mut remaining_waypoints = remaining_waypoints.clone(); @@ -142,7 +155,7 @@ impl NavigationController { }; let progress = calculate_trip_progress( - &(*snapped_user_location).into(), + &(updated_snapped_user_location).into(), &linestring, &remaining_steps, ); @@ -157,8 +170,8 @@ impl NavigationController { .and_then(|index| current_step.get_annotation_at_current_index(index)); TripState::Navigating { - current_step_geometry_index: *current_step_geometry_index, - snapped_user_location: *snapped_user_location, + current_step_geometry_index: updated_current_step_geometry_index, + snapped_user_location: updated_snapped_user_location, remaining_steps, remaining_waypoints, progress,