Improves Lane Handling for Multi-Hop Roundabout Instruction

This changeset implements Lane Anticipation on roundabouts, delimited
by enter / leave step pairs. It does not handle lane anticipation
within a roundabout.

Lane anticipation happens on the granularity of a valid roundbaout:

We discard partial roundabout (enter without exit or exit without
enter) or data issues (no roundabout, exit before enter).

Related:

- https://github.com/Project-OSRM/osrm-backend/issues/2626 for lanes
  within a roundabout

- https://github.com/Project-OSRM/osrm-backend/issues/2625 for handling
  going straight in lane anticipation

include/engine/guidance/toolkit.hpp

@@ -2,10 +2,13 @@
 #define OSRM_ENGINE_GUIDANCE_TOOLKIT_HPP_
 
 #include "extractor/guidance/turn_instruction.hpp"
-#include "util/guidance/toolkit.hpp"
+#include "engine/guidance/route_step.hpp"
 #include "util/bearing.hpp"
+#include "util/guidance/toolkit.hpp"
 
 #include <algorithm>
+#include <iterator>
+#include <utility>
 
 namespace osrm
 {
@@ -40,6 +43,38 @@ inline extractor::guidance::DirectionModifier::Enum angleToDirectionModifier(con
     return extractor::guidance::DirectionModifier::Left;
 }
 
+// Runs fn on RouteStep sub-ranges determined to be roundabouts.
+// The function fn is getting called with a roundabout range as in: [enter, .., leave].
+//
+// The following situations are taken care for (i.e. we discard them):
+//  - partial roundabout: enter without exit or exit without enter
+//  - data issues: no roundabout, exit before enter
+template <typename Iter, typename Fn> inline Fn forEachRoundabout(Iter first, Iter last, Fn fn)
+{
+    while (first != last)
+    {
+        const auto enter = std::find_if(first, last, [](const RouteStep &step) {
+            return entersRoundabout(step.maneuver.instruction);
+        });
+
+        // enter has to come before leave, otherwise: faulty data / partial roundabout, skip those
+        const auto leave = std::find_if(enter, last, [](const RouteStep &step) {
+            return leavesRoundabout(step.maneuver.instruction);
+        });
+
+        // No roundabouts, or partial one (like start / end inside a roundabout)
+        if (enter == last || leave == last)
+            break;
+
+        (void)fn(std::make_pair(enter, leave));
+
+        // Skip to first step after the currently handled enter / leave pair
+        first = std::next(leave);
+    }
+
+    return fn;
+}
+
 } // namespace guidance
 } // namespace engine
 } // namespace osrm

include/util/debug.hpp

@@ -21,10 +21,13 @@ inline void print(const engine::guidance::RouteStep &step)
 {
     std::cout << static_cast<int>(step.maneuver.instruction.type) << " "
               << static_cast<int>(step.maneuver.instruction.direction_modifier) << "  "
-              << static_cast<int>(step.maneuver.waypoint_type) << " Duration: " << step.duration
-              << " Distance: " << step.distance << " Geometry: " << step.geometry_begin << " "
-              << step.geometry_end << " exit: " << step.maneuver.exit
-              << " Intersections: " << step.intersections.size() << " [";
+              << static_cast<int>(step.maneuver.waypoint_type) << " "
+              << " Lanes: (" << static_cast<int>(step.maneuver.lanes.lanes_in_turn) << ", "
+              << static_cast<int>(step.maneuver.lanes.first_lane_from_the_right) << ")"
+              << " Duration: " << step.duration << " Distance: " << step.distance
+              << " Geometry: " << step.geometry_begin << " " << step.geometry_end
+              << " exit: " << step.maneuver.exit << " Intersections: " << step.intersections.size()
+              << " [";
 
     for (const auto &intersection : step.intersections)
     {