Fix incorrect geometry index after advancing

After advancing the step, the geometry index sometimes incorrectly refers to an index in the previous RouteStep instead of the current one. This patch recomputes the geometry index when the RouteSteps change.
stadiamaps · Nov 12, 2024 · 404fc4b · 404fc4b
1 parent f5790e2
commit 404fc4b
Showing 1 changed file with 27 additions and 14 deletions.
diff --git 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,
+                                    &current_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,