u-turn logic threshold fix

app/src/main/java/com/mapbox/services/android/navigation/testapp/activity/MockNavigationActivity.java

@@ -34,6 +34,7 @@
 import com.mapbox.services.android.navigation.v5.navigation.NavigationConstants;
 import com.mapbox.services.android.navigation.v5.navigation.NavigationEventListener;
 import com.mapbox.services.android.navigation.v5.navigation.NavigationRoute;
+import com.mapbox.services.android.navigation.v5.offroute.OffRouteListener;
 import com.mapbox.services.android.navigation.v5.routeprogress.ProgressChangeListener;
 import com.mapbox.services.android.navigation.v5.routeprogress.RouteProgress;
 import com.mapbox.services.android.telemetry.location.LocationEngine;
@@ -52,7 +53,8 @@
 import timber.log.Timber;
 
 public class MockNavigationActivity extends AppCompatActivity implements OnMapReadyCallback,
-  MapboxMap.OnMapClickListener, ProgressChangeListener, NavigationEventListener, MilestoneEventListener {
+  MapboxMap.OnMapClickListener, ProgressChangeListener, NavigationEventListener,
+  MilestoneEventListener, OffRouteListener {
 
   private static final int BEGIN_ROUTE_MILESTONE = 1001;
 
@@ -112,6 +114,7 @@ public void onStartRouteClick() {
       navigation.addNavigationEventListener(this);
       navigation.addProgressChangeListener(this);
       navigation.addMilestoneEventListener(this);
+      navigation.addOffRouteListener(this);
 
       ((MockLocationEngine) locationEngine).setRoute(route);
       navigation.setLocationEngine(locationEngine);
@@ -149,7 +152,7 @@ public void onMapReady(MapboxMap mapboxMap) {
     mapboxMap.setOnMapClickListener(this);
     Snackbar.make(mapView, "Tap map to place waypoint", BaseTransientBottomBar.LENGTH_LONG).show();
 
-    locationEngine = new MockLocationEngine(1000, 50, false);
+    locationEngine = new MockLocationEngine(1000, 50, true);
     mapboxMap.setLocationSource(locationEngine);
 
     newOrigin();
@@ -196,10 +199,10 @@ private void calculateRoute() {
       public void onResponse(Call<DirectionsResponse> call, Response<DirectionsResponse> response) {
         Timber.d("Url: %s", call.request().url().toString());
         if (response.body() != null) {
-          if (response.body().getRoutes().size() > 0) {
-            DirectionsRoute route = response.body().getRoutes().get(0);
-            MockNavigationActivity.this.route = route;
-            navigationMapRoute.addRoute(route);
+          if (!response.body().getRoutes().isEmpty()) {
+            DirectionsRoute directionsRoute = response.body().getRoutes().get(0);
+            MockNavigationActivity.this.route = directionsRoute;
+            navigationMapRoute.addRoute(directionsRoute);
 
             /*for (LegStep step: route.getLegs().get(0).getSteps()) {
               mapboxMap.addMarker(new MarkerOptions().position(new LatLng(
@@ -258,6 +261,11 @@ public void onRunning(boolean running) {
     }
   }
 
+  @Override
+  public void userOffRoute(Location location) {
+    Toast.makeText(this, "off-route called", Toast.LENGTH_LONG).show();
+  }
+
   @Override
   public void onProgressChange(Location location, RouteProgress routeProgress) {
     locationLayerPlugin.forceLocationUpdate(location);

app/src/main/java/com/mapbox/services/android/navigation/testapp/activity/RerouteActivity.java

@@ -90,7 +90,7 @@ public void onMapReady(MapboxMap mapboxMap) {
     navigation.setLocationEngine(locationEngine);
 
     // Acquire the navigation's route
-    getRoute(origin, destination);
+    getRoute(origin, destination, null);
   }
 
   @Override
@@ -118,7 +118,7 @@ public void onRunning(boolean running) {
   @Override
   public void userOffRoute(Location location) {
     Position newOrigin = Position.fromLngLat(location.getLongitude(), location.getLatitude());
-    getRoute(newOrigin, destination);
+    getRoute(newOrigin, destination, location.getBearing());
     Timber.d("offRoute");
     mapboxMap.addMarker(new MarkerOptions().position(new LatLng(location.getLatitude(), location.getLongitude())));
   }
@@ -133,7 +133,7 @@ public void onProgressChange(Location location, RouteProgress routeProgress) {
   @Override
   public void onResponse(Call<DirectionsResponse> call, Response<DirectionsResponse> response) {
     if (response.body() != null) {
-      if (response.body().getRoutes().size() > 0) {
+      if (!response.body().getRoutes().isEmpty()) {
         DirectionsRoute route = response.body().getRoutes().get(0);
         // First run
         drawRoute(route);
@@ -145,14 +145,16 @@ public void onResponse(Call<DirectionsResponse> call, Response<DirectionsRespons
     }
   }
 
-  private void getRoute(Position origin, Position destination) {
-    NavigationRoute navigationRoute = NavigationRoute.builder()
+  private void getRoute(Position origin, Position destination, Float bearing) {
+    NavigationRoute.Builder navigationRouteBuilder = NavigationRoute.builder()
       .origin(origin)
       .destination(destination)
-      .accessToken(Mapbox.getAccessToken())
-      .build();
+      .accessToken(Mapbox.getAccessToken());
 
-    navigationRoute.getRoute(this);
+    if (bearing != null) {
+      navigationRouteBuilder.addBearing(bearing, 90);
+    }
+    navigationRouteBuilder.build().getRoute(this);
   }
 
   @Override
@@ -168,7 +170,7 @@ private void drawRoute(DirectionsRoute route) {
       points.add(new LatLng(position.getLatitude(), position.getLongitude()));
     }
 
-    if (points.size() > 0) {
+    if (!points.isEmpty()) {
 
       if (polyline != null) {
         mapboxMap.removePolyline(polyline);

libandroid-navigation/src/main/java/com/mapbox/services/android/navigation/v5/navigation/NavigationConstants.java

@@ -121,6 +121,8 @@ private NavigationConstants() {
    */
   static final double METERS_REMAINING_TILL_ARRIVAL = 40;
 
+  public static final double MINIMUM_BACKUP_DISTANCE_FOR_OFF_ROUTE = 50;
+
   // Bundle variable keys
   public static final String NAVIGATION_VIEW_ORIGIN_LAT_KEY = "origin_lat";
   public static final String NAVIGATION_VIEW_ORIGIN_LNG_KEY = "origin_long";

libandroid-navigation/src/main/java/com/mapbox/services/android/navigation/v5/navigation/NavigationEngine.java

@@ -34,8 +34,6 @@
 class NavigationEngine extends HandlerThread implements Handler.Callback {
 
   private static final String THREAD_NAME = "NavThread";
-
-  private RingBuffer<Integer> recentDistancesFromManeuverInMeters;
   private RouteProgress previousRouteProgress;
   private List<Position> stepPositions;
   private NavigationIndices indices;
@@ -48,7 +46,6 @@ class NavigationEngine extends HandlerThread implements Handler.Callback {
     this.responseHandler = responseHandler;
     this.callback = callback;
     indices = NavigationIndices.create(0, 0);
-    recentDistancesFromManeuverInMeters = new RingBuffer<>(3);
   }
 
   void queueTask(int msgIdentifier, NewLocationModel newLocationModel) {
@@ -68,11 +65,11 @@ public boolean handleMessage(Message msg) {
 
   private void handleRequest(final NewLocationModel newLocationModel) {
     final RouteProgress routeProgress = generateNewRouteProgress(
-      newLocationModel.mapboxNavigation(), newLocationModel.location());
+      newLocationModel.mapboxNavigation(), newLocationModel.location(),
+      newLocationModel.recentDistancesFromManeuverInMeters());
     final List<Milestone> milestones = checkMilestones(
       previousRouteProgress, routeProgress, newLocationModel.mapboxNavigation());
-    final boolean userOffRoute = isUserOffRoute(newLocationModel, routeProgress,
-      recentDistancesFromManeuverInMeters);
+    final boolean userOffRoute = isUserOffRoute(newLocationModel, routeProgress);
     final Location location = !userOffRoute && newLocationModel.mapboxNavigation().options().snapToRoute()
       ? getSnappedLocation(newLocationModel.mapboxNavigation(), newLocationModel.location(),
       routeProgress, stepPositions)
@@ -90,7 +87,8 @@ public void run() {
     });
   }
 
-  private RouteProgress generateNewRouteProgress(MapboxNavigation mapboxNavigation, Location location) {
+  private RouteProgress generateNewRouteProgress(MapboxNavigation mapboxNavigation, Location location,
+                                                 RingBuffer recentDistances) {
     DirectionsRoute directionsRoute = mapboxNavigation.getRoute();
     MapboxNavigationOptions options = mapboxNavigation.options();
 
@@ -144,7 +142,7 @@ private RouteProgress generateNewRouteProgress(MapboxNavigation mapboxNavigation
         legDistanceRemaining, indices.legIndex(), directionsRoute);
 
       // Remove all distance values from recentDistancesFromManeuverInMeters
-      recentDistancesFromManeuverInMeters.clear();
+      recentDistances.clear();
     }
 
     // Create a RouteProgress.create object using the latest user location
@@ -159,8 +157,8 @@ private RouteProgress generateNewRouteProgress(MapboxNavigation mapboxNavigation
   }
 
   /**
-   * Check used to determine if navigation is starting for the first time (previousRouteProgress is null).
-   * Or, a new route has been found (in off-route scenarios).
+   * Check used to determine if navigation is starting for the first time (previousRouteProgress is
+   * null). Or, a new route has been found (in off-route scenarios).
    *
    * @param directionsRoute to check against the current route
    * @return true if starting navigation for the first time

libandroid-navigation/src/main/java/com/mapbox/services/android/navigation/v5/navigation/NavigationHelper.java

@@ -6,7 +6,6 @@
 import com.mapbox.services.android.navigation.v5.offroute.OffRoute;
 import com.mapbox.services.android.navigation.v5.routeprogress.RouteProgress;
 import com.mapbox.services.android.navigation.v5.snap.Snap;
-import com.mapbox.services.android.navigation.v5.utils.RingBuffer;
 import com.mapbox.services.android.telemetry.utils.MathUtils;
 import com.mapbox.services.api.directions.v5.models.DirectionsRoute;
 import com.mapbox.services.api.directions.v5.models.LegStep;
@@ -181,12 +180,11 @@ static List<Milestone> checkMilestones(RouteProgress previousRouteProgress,
     return milestones;
   }
 
-  static boolean isUserOffRoute(NewLocationModel newLocationModel, RouteProgress routeProgress,
-                                RingBuffer<Integer> recentDistancesFromManeuverInMeters) {
+  static boolean isUserOffRoute(NewLocationModel newLocationModel, RouteProgress routeProgress) {
     OffRoute offRoute = newLocationModel.mapboxNavigation().getOffRouteEngine();
     return offRoute.isUserOffRoute(newLocationModel.location(), routeProgress,
       newLocationModel.mapboxNavigation().options(),
-      recentDistancesFromManeuverInMeters);
+      newLocationModel.recentDistancesFromManeuverInMeters());
   }
 
   static Location getSnappedLocation(MapboxNavigation mapboxNavigation, Location location,

libandroid-navigation/src/main/java/com/mapbox/services/android/navigation/v5/navigation/NavigationRoute.java

@@ -65,7 +65,6 @@ public void getRoute(@NonNull List<Callback<DirectionsResponse>> callbacks) {
 
   MapboxDirections getDirectionsRequest() {
 
-
     MapboxDirections.Builder builder = new MapboxDirections.Builder()
       .setUser(user())
       .setProfile(profile())
@@ -78,6 +77,7 @@ MapboxDirections getDirectionsRequest() {
       .setGeometry(DirectionsCriteria.GEOMETRY_POLYLINE6)
       .setOverview(DirectionsCriteria.OVERVIEW_FULL)
       .setSteps(true)
+      .setContinueStraight(true)
       .setRoundaboutExits(true);
 
     if (!bearings().isEmpty()) {

libandroid-navigation/src/main/java/com/mapbox/services/android/navigation/v5/navigation/NavigationService.java

@@ -14,6 +14,7 @@
 import com.mapbox.services.android.navigation.R;
 import com.mapbox.services.android.navigation.v5.milestone.Milestone;
 import com.mapbox.services.android.navigation.v5.routeprogress.RouteProgress;
+import com.mapbox.services.android.navigation.v5.utils.RingBuffer;
 import com.mapbox.services.android.telemetry.location.LocationEngine;
 import com.mapbox.services.android.telemetry.location.LocationEngineListener;
 import com.mapbox.services.api.directions.v5.models.DirectionsRoute;
@@ -43,6 +44,7 @@ public class NavigationService extends Service implements LocationEngineListener
   // Message id used when a new location update occurs and we send to the thread.
   private static final int MSG_LOCATION_UPDATED = 1001;
 
+  private RingBuffer<Integer> recentDistancesFromManeuverInMeters;
   private final IBinder localBinder = new LocalBinder();
   private NavigationNotification navNotificationManager;
   private long timeIntervalSinceLastOffRoute;
@@ -64,6 +66,7 @@ public void onCreate() {
     thread = new NavigationEngine(new Handler(), this);
     thread.start();
     thread.prepareHandler();
+    recentDistancesFromManeuverInMeters = new RingBuffer<>(3);
   }
 
   /**
@@ -148,7 +151,8 @@ void acquireLocationEngine() {
   private void forceLocationUpdate() {
     Location lastLocation = locationEngine.getLastLocation();
     if (lastLocation != null) {
-      thread.queueTask(MSG_LOCATION_UPDATED, NewLocationModel.create(lastLocation, mapboxNavigation));
+      thread.queueTask(MSG_LOCATION_UPDATED, NewLocationModel.create(lastLocation, mapboxNavigation,
+        recentDistancesFromManeuverInMeters));
     }
   }
 
@@ -164,7 +168,8 @@ public void onLocationChanged(Location location) {
     Timber.d("onLocationChanged");
     if (location != null) {
       if (validLocationUpdate(location)) {
-        thread.queueTask(MSG_LOCATION_UPDATED, NewLocationModel.create(location, mapboxNavigation));
+        thread.queueTask(MSG_LOCATION_UPDATED, NewLocationModel.create(location, mapboxNavigation,
+          recentDistancesFromManeuverInMeters));
       }
     }
   }
@@ -230,6 +235,7 @@ public void onUserOffRoute(Location location, boolean userOffRoute) {
     if (userOffRoute) {
       if (location.getTime() > timeIntervalSinceLastOffRoute
         + TimeUnit.SECONDS.toMillis(mapboxNavigation.options().secondsBeforeReroute())) {
+        recentDistancesFromManeuverInMeters.clear();
         mapboxNavigation.getEventDispatcher().onUserOffRoute(location);
         timeIntervalSinceLastOffRoute = location.getTime();
       }

libandroid-navigation/src/main/java/com/mapbox/services/android/navigation/v5/navigation/NewLocationModel.java

@@ -3,15 +3,20 @@
 import android.location.Location;
 
 import com.google.auto.value.AutoValue;
+import com.mapbox.services.android.navigation.v5.utils.RingBuffer;
 
 @AutoValue
 abstract class NewLocationModel {
 
-  static NewLocationModel create(Location location, MapboxNavigation mapboxNavigation) {
-    return new AutoValue_NewLocationModel(location, mapboxNavigation);
+  static NewLocationModel create(Location location, MapboxNavigation mapboxNavigation,
+                                 RingBuffer recentDistancesFromManeuverInMeters) {
+    return new AutoValue_NewLocationModel(location, mapboxNavigation,
+      recentDistancesFromManeuverInMeters);
   }
 
   abstract Location location();
 
   abstract MapboxNavigation mapboxNavigation();
+
+  abstract RingBuffer recentDistancesFromManeuverInMeters();
 }

libandroid-navigation/src/main/java/com/mapbox/services/android/navigation/v5/offroute/OffRouteDetector.java

@@ -15,6 +15,8 @@
 import com.mapbox.services.commons.geojson.Point;
 import com.mapbox.services.commons.models.Position;
 
+import static com.mapbox.services.android.navigation.v5.navigation.NavigationConstants.MINIMUM_BACKUP_DISTANCE_FOR_OFF_ROUTE;
+
 public class OffRouteDetector extends OffRoute {
 
   /**
@@ -77,12 +79,19 @@ private static boolean movingAwayFromManeuver(RouteProgress routeProgress,
                                                 RingBuffer<Integer> recentDistancesFromManeuverInMeters,
                                                 Position futurePosition) {
 
+    if (routeProgress.currentLegProgress().upComingStep() == null) {
+      return false;
+    }
+
     double userDistanceToManeuver = TurfMeasurement.distance(
-      routeProgress.currentLegProgress().currentStep().getManeuver().asPosition(),
+      routeProgress.currentLegProgress().upComingStep().getManeuver().asPosition(),
       futurePosition, TurfConstants.UNIT_METERS
     );
 
-    if (recentDistancesFromManeuverInMeters.size() >= 3) {
+    if (!recentDistancesFromManeuverInMeters.isEmpty()
+      && recentDistancesFromManeuverInMeters.peekLast()
+      - recentDistancesFromManeuverInMeters.peekFirst() < MINIMUM_BACKUP_DISTANCE_FOR_OFF_ROUTE
+      && recentDistancesFromManeuverInMeters.size() >= 3) {
       // User's moving away from maneuver position, thus offRoute.
       return true;
     }