diff --git a/features/guidance/anticipate-lanes.feature b/features/guidance/anticipate-lanes.feature new file mode 100644 index 000000000..5c80cf635 --- /dev/null +++ b/features/guidance/anticipate-lanes.feature @@ -0,0 +1,314 @@ +@routing @guidance @turn-lanes +Feature: Turn Lane Guidance + + Background: + Given the profile "car" + Given a grid size of 20 meters + + @anticipate + Scenario: Anticipate Lane Change for subsequent multi-lane intersections + Given the node map + | a | | b | | x | | | + | | | | | | | | + | | | c | | d | | z | + | | | | | | | | + | | | y | | e | | | + + And the ways + | nodes | turn:lanes:forward | + | ab | through\|right&right&right | + | bx | | + | bc | left\|left&through | + | cd | through\|right | + | cy | | + | dz | | + | de | | + + When I route I should get + | waypoints | route | turns | lanes | # | + | a,d | ab,bc,cd,cd | depart,turn right,turn left,arrive | ,1 2,1 2, | 2 hops | + | a,e | ab,bc,cd,de,de | depart,turn right,turn left,turn right,arrive | ,1,1,0, | 3 hops | + + @anticipate + Scenario: Anticipate Lane Change for quick same direction turns, staying on the same street + Given the node map + | a | | b | x | + | | | | | + | | | c | | + | | | | | + | e | | d | y | + + And the ways + | nodes | turn:lanes:forward | turn:lanes:backward | name | + | ab | through\|right&right | | MySt | + | bx | | | XSt | + | bc | | left\|right | MySt | + | cd | left\|right | through\|through | MySt | + | de | | left\|left&through | MySt | + | dy | | | YSt | + + When I route I should get + | waypoints | route | turns | lanes | + | a,e | MySt,MySt,MySt,MySt | depart,continue right,end of road right,arrive | ,0,0, | + | e,a | MySt,MySt,MySt,MySt | depart,continue left,end of road left,arrive | ,2,1, | + + @anticipate + Scenario: Anticipate Lane Change for quick same direction turns, changing between streets + Given the node map + | a | | b | x | + | | | | | + | | | c | | + | | | | | + | e | | d | y | + + And the ways + | nodes | turn:lanes:forward | turn:lanes:backward | name | + | ab | through\|right&right | | AXSt | + | bx | | | AXSt | + | bc | | left\|right | BDSt | + | cd | left\|right | through\|through | BDSt | + | de | | left\|left&through | EYSt | + | dy | | | EYSt | + + When I route I should get + | waypoints | route | turns | lanes | + | a,e | AXSt,BDSt,EYSt,EYSt | depart,turn right,end of road right,arrive | ,0,0, | + | e,a | EYSt,BDSt,AXSt,AXSt | depart,turn left,end of road left,arrive | ,2,1, | + + + @anticipate + Scenario: Anticipate Lane Change for quick turns during a merge + Given the node map + | a | | | | | + | x | b | | c | y | + | | | | | d | + + And the ways + | nodes | turn:lanes:forward | name | highway | oneway | + | ab | slight_left\|slight_left | On | motorway_link | yes | + | xb | | Hwy | motorway | | + | bc | through\|slight_right | Hwy | motorway | | + | cd | | Off | motorway_link | yes | + | cy | | Hwy | motorway | | + + When I route I should get + | waypoints | route | turns | lanes | + | a,d | On,Hwy,Off,Off | depart,merge slight right,off ramp right,arrive | ,0,0, | + + + @anticipate + Scenario: Schoenefelder Kreuz + # https://www.openstreetmap.org/way/264306388#map=16/52.3202/13.5568 + Given the node map + | a | b | x | | | i | + | | | c | d | | | + | | | | | | j | + + And the ways + | nodes | turn:lanes:forward | lanes | highway | oneway | name | + | ab | none\|none&none&slight_right&slight_right | 5 | motorway | | abx | + | bx | | 3 | motorway | | abx | + | bc | | 2 | motorway_link | yes | bcd | + | cd | slight_left\|slight_left;slight_right&slight_right | 3 | motorway_link | yes | bcd | + | di | slight_left\|slight_right | 2 | motorway_link | yes | di | + | dj | | 2 | motorway_link | yes | dj | + + When I route I should get + | waypoints | route | turns | lanes | + | a,i | abx,bcd,di,di | depart,off ramp right,fork slight left,arrive | ,0 1,1 2, | + | a,j | abx,bcd,dj,dj | depart,off ramp right,fork slight right,arrive | ,0 1,0 1, | + + + @anticipate + Scenario: Kreuz Oranienburg + # https://www.openstreetmap.org/way/4484007#map=18/52.70439/13.20269 + Given the node map + | i | | | | | a | + | j | | c | b | | x | + + And the ways + | nodes | turn:lanes:forward | lanes | highway | oneway | name | + | ab | | 1 | motorway_link | yes | ab | + | xb | | 1 | motorway_link | yes | xbcj | + | bc | none\|slight_right | 2 | motorway_link | yes | xbcj | + | ci | | 1 | motorway_link | yes | ci | + | cj | | 1 | motorway_link | yes | xbcj | + + When I route I should get + | waypoints | route | turns | lanes | + | a,i | ab,xbcj,ci,ci | depart,merge slight left,turn slight right,arrive | ,,0, | + | a,j | ab,xbcj,xbcj,xbcj | depart,merge slight left,use lane straight,arrive | ,,1, | + + + @anticipate + Scenario: Lane anticipation for fan-in + Given the node map + | a | | b | | x | | | + | | | | | | | | + | | | c | | d | | z | + | | | | | | | | + | | | y | | e | | | + + And the ways + | nodes | turn:lanes:forward | name | + | ab | through\|right&right&right | abx | + | bx | | abx | + | bc | left\|left&through | bcy | + | cy | | bcy | + | cd | through\|right | cdz | + | dz | | cdz | + | de | | de | + + When I route I should get + | waypoints | route | turns | lanes | + | a,e | abx,bcy,cdz,de,de | depart,turn right,turn left,turn right,arrive | ,1,1,0, | + + + @anticipate + Scenario: Lane anticipation for fan-out + Given the node map + | a | | b | | x | | | + | | | | | | | | + | | | c | | d | | z | + | | | | | | | | + | | | y | | e | | | + + And the ways + | nodes | turn:lanes:forward | name | + | ab | through\|right | abx | + | bx | | abx | + | bc | left\|left&through | bcy | + | cy | | bcy | + | cd | through\|right&right&right | cdz | + | dz | | cdz | + | de | | de | + + When I route I should get + | waypoints | route | turns | lanes | + | a,e | abx,bcy,cdz,de,de | depart,turn right,turn left,turn right,arrive | ,0,1 2,0 1 2, | + + + @anticipate + Scenario: Lane anticipation for fan-in followed by fan-out + Given the node map + | a | | b | | x | | | + | | | | | | | | + | | | c | | d | | z | + | | | | | | | | + | | | y | | e | | | + + And the ways + | nodes | turn:lanes:forward | name | + | ab | through\|right&right&right | abx | + | bx | | abx | + | bc | left\|left&through | bcy | + | cy | | bcy | + | cd | through\|right&right&right | cdz | + | dz | | cdz | + | de | | de | + + When I route I should get + | waypoints | route | turns | lanes | + | a,e | abx,bcy,cdz,de,de | depart,turn right,turn left,turn right,arrive | ,1 2,1 2,0 1 2, | + + + @anticipate + Scenario: Lane anticipation for fan-out followed by fan-in + Given the node map + | a | | b | | x | | | + | | | | | | | | + | | | c | | d | | z | + | | | | | | | | + | | | y | | e | | | + + And the ways + | nodes | turn:lanes:forward | name | + | ab | through\|right | abx | + | bx | | abx | + | bc | left\|left&through | bcy | + | cy | | bcy | + | cd | through\|right | cdz | + | dz | | cdz | + | de | | de | + + When I route I should get + | waypoints | route | turns | lanes | + | a,e | abx,bcy,cdz,de,de | depart,turn right,turn left,turn right,arrive | ,0,1,0, | + + + @anticipate + Scenario: Lane anticipation for multiple hops with same number of lanes + Given the node map + | a | | b | | x | | | + | | | | | | | | + | | | c | | d | | z | + | | | | | | | | + | | | y | | e | | f | + | | | | | | | | + | | | | | w | | | + + And the ways + | nodes | turn:lanes:forward | name | + | ab | through\|right&right&right | abx | + | bx | | abx | + | bc | left\|left&through | bcy | + | cy | | bcy | + | cd | through\|right&right | cdz | + | dz | | cdz | + | de | left\|through | dew | + | ew | | dew | + | ef | | ef | + + When I route I should get + | waypoints | route | turns | lanes | + | a,f | abx,bcy,cdz,dew,ef,ef | depart,turn right,turn left,turn right,turn left,arrive | ,1,1,1,1, | + + @anticipate + Scenario: Tripple Right keeping Left + Given the node map + | a | | | | b | | i | + | | | | | | | | + | | | | | | | | + | f | | e | | | | g | + | | | | | | | | + | | | | | | | | + | | j | d | | c | | | + | | | | | h | | | + + And the ways + | nodes | turn:lanes:forward | highway | name | + | abi | \|&right&right | primary | start | + | bch | \|&right&right | primary | first | + | cdj | \|&right&right | primary | second | + | de | left\|right&right | secondary | third | + | feg | | tertiary | fourth | + + When I route I should get + | waypoints | route | turns | lanes | + | a,f | start,first,second,third,fourth,fourth | depart,turn right,turn right,turn right,end of road left,arrive | ,2,2,2,2, | + | a,g | start,first,second,third,fourth,fourth | depart,turn right,turn right,turn right,end of road right,arrive | ,0 1,0 1,0 1,0 1, | + + @anticipate + Scenario: Tripple Left keeping Right + Given the node map + | i | | b | | | | a | + | | | | | | | | + | | | | | | | | + | g | | | | e | | f | + | | | | | | | | + | | | | | | | | + | | | c | | d | j | | + | | | h | | | | | + + And the ways + | nodes | turn:lanes:forward | highway | name | + | abi | left\|left&& | primary | start | + | bch | left\|left&& | primary | first | + | cdj | left\|left&& | primary | second | + | de | left\|left&right | secondary | third | + | feg | | tertiary | fourth | + + When I route I should get + | waypoints | route | turns | lanes | + | a,f | start,first,second,third,fourth,fourth | depart,turn left,turn left,turn left,end of road right,arrive | ,2,2,2,2, | + | a,g | start,first,second,third,fourth,fourth | depart,turn left,turn left,turn left,end of road left,arrive | ,0 1,0 1,0 1,0 1, | diff --git a/features/guidance/turn-lanes.feature b/features/guidance/turn-lanes.feature index 60150a8f1..3e5b69823 100644 --- a/features/guidance/turn-lanes.feature +++ b/features/guidance/turn-lanes.feature @@ -290,26 +290,6 @@ Feature: Turn Lane Guidance | a,e | road,through,through | depart,new name straight,arrive | ,1, | | a,f | road,right,right | depart,turn right,arrive | ,0, | - Scenario: Anticipate Lane Change - Given the node map - | a | | b | | x | - | | | | | | - | | | c | | d | - | | | | | | - | | | y | | | - - And the ways - | nodes | turn:lanes:forward | turn:lanes:backward | - | ab | through\|right&right | | - | bx | | left\|left&through | - | bc | left\|through | left\|right | - | cd | | left\|right | - | cy | | | - - When I route I should get - | waypoints | route | turns | lanes | - | d,a | cd,bc,ab,ab | depart,end of road right,end of road left,arrive | ,0,1, | - Scenario: Turn at a traffic light Given the node map | a | b | c | d | diff --git a/include/engine/api/route_api.hpp b/include/engine/api/route_api.hpp index 76ff9a790..31724fe5e 100644 --- a/include/engine/api/route_api.hpp +++ b/include/engine/api/route_api.hpp @@ -149,6 +149,7 @@ class RouteAPI : public BaseAPI leg_geometry, phantoms.source_phantom, phantoms.target_phantom); + leg.steps = guidance::anticipateLaneChange(std::move(leg.steps)); leg_geometry = guidance::resyncGeometry(std::move(leg_geometry), leg.steps); } diff --git a/include/engine/guidance/post_processing.hpp b/include/engine/guidance/post_processing.hpp index 16d383df6..37adf7637 100644 --- a/include/engine/guidance/post_processing.hpp +++ b/include/engine/guidance/post_processing.hpp @@ -43,6 +43,11 @@ std::vector buildIntersections(std::vector steps); // remove steps invalidated by post-processing std::vector removeNoTurnInstructions(std::vector steps); +// Constrains lanes for multi-hop situations where lane changes depend on earlier ones. +// Instead of forcing users to change lanes rapidly in a short amount of time, +// we anticipate lane changes emitting only matching lanes early on. +std::vector anticipateLaneChange(std::vector steps); + // postProcess will break the connection between the leg geometry // for which a segment is supposed to represent exactly the coordinates // between routing maneuvers and the route steps itself. diff --git a/include/util/group_by.hpp b/include/util/group_by.hpp new file mode 100644 index 000000000..db8a312e8 --- /dev/null +++ b/include/util/group_by.hpp @@ -0,0 +1,40 @@ +#ifndef OSRM_GROUP_BY +#define OSRM_GROUP_BY + +#include +#include + +namespace osrm +{ +namespace util +{ + +// Runs fn on consecutive items in sub-ranges determined by pred. +// +// Example: +// vector v{1,2,2,2,3,4,4}; +// group_by(first, last, even, print); +// >>> 2,2,2 +// >>> 4,4 +// +// Note: this mimics Python's itertools.groupby +template +Fn group_by(Iter first, Iter last, Pred pred, Fn fn) +{ + while (first != last) + { + first = std::find_if(first, last, pred); + auto next = std::find_if_not(first, last, pred); + + (void)fn(std::make_pair(first, next)); + + first = next; + } + + return fn; +} + +} // ns util +} // ns osrm + +#endif diff --git a/src/engine/guidance/post_processing.cpp b/src/engine/guidance/post_processing.cpp index 373ee13e0..48c9065a2 100644 --- a/src/engine/guidance/post_processing.cpp +++ b/src/engine/guidance/post_processing.cpp @@ -4,6 +4,8 @@ #include "engine/guidance/assemble_steps.hpp" #include "engine/guidance/toolkit.hpp" +#include "util/for_each_pair.hpp" +#include "util/group_by.hpp" #include "util/guidance/toolkit.hpp" #include @@ -13,6 +15,7 @@ #include #include #include +#include #include #include @@ -306,7 +309,6 @@ void closeOffRoundabout(const bool on_roundabout, } propagation_step.name = destination_name; - ; propagation_step.name_id = destinatino_name_id; invalidateStep(steps[propagation_index + 1]); break; @@ -1036,6 +1038,94 @@ std::vector assignRelativeLocations(std::vector steps, return steps; } +std::vector anticipateLaneChange(std::vector steps) +{ + const constexpr auto MIN_DURATION_NEEDED_FOR_LANE_CHANGE = 15.; + + // Postprocessing does not strictly guarantee for only turns + const auto is_turn = [](const RouteStep &step) { + return step.maneuver.instruction.type != TurnType::NewName && + step.maneuver.instruction.type != TurnType::Notification; + }; + + const auto is_quick = [](const RouteStep &step) { + return step.duration < MIN_DURATION_NEEDED_FOR_LANE_CHANGE; + }; + + const auto is_quick_turn = [&](const RouteStep &step) { + return is_turn(step) && is_quick(step); + }; + + // Determine range of subsequent quick turns, candidates for possible lane anticipation + using StepIter = decltype(steps)::iterator; + using StepIterRange = std::pair; + + std::vector subsequent_quick_turns; + + const auto keep_turn_range = [&](StepIterRange range) { + if (std::distance(range.first, range.second) > 1) + subsequent_quick_turns.push_back(std::move(range)); + }; + + util::group_by(begin(steps), end(steps), is_quick_turn, keep_turn_range); + + // Walk backwards over all turns, constraining possible turn lanes. + // Later turn lanes constrain earlier ones: we have to anticipate lane changes. + const auto constrain_lanes = [](const StepIterRange &turns) { + const std::reverse_iterator rev_first{turns.second}; + const std::reverse_iterator rev_last{turns.first}; + + // We're walking backwards over all adjacent turns: + // the current turn lanes constrain the lanes we have to take in the previous turn. + util::for_each_pair(rev_first, rev_last, [](RouteStep ¤t, RouteStep &previous) { + const auto current_inst = current.maneuver.instruction; + const auto current_lanes = current_inst.lane_tupel; + + // Constrain the previous turn's lanes + auto &previous_inst = previous.maneuver.instruction; + auto &previous_lanes = previous_inst.lane_tupel; + + // Lane mapping (N:M) from previous lanes (N) to current lanes (M), with: + // N > M, N > 1 fan-in situation, constrain N lanes to min(N,M) shared lanes + // otherwise nothing to constrain + const bool lanes_to_constrain = previous_lanes.lanes_in_turn > 1; + const bool lanes_fan_in = previous_lanes.lanes_in_turn > current_lanes.lanes_in_turn; + + if (!lanes_to_constrain || !lanes_fan_in) + return; + + // In case there is no lane information we work with one artificial lane + const auto current_adjusted_lanes = std::max(current_lanes.lanes_in_turn, LaneID{1}); + + const auto num_shared_lanes = std::min(current_adjusted_lanes, // + previous_lanes.lanes_in_turn); + + if (isRightTurn(current_inst)) + { + // Current turn is right turn, already keep right during the previous turn. + // This implies constraining the leftmost lanes in the previous turn step. + previous_lanes = {num_shared_lanes, previous_lanes.first_lane_from_the_right}; + } + else if (isLeftTurn(current_inst)) + { + // Current turn is left turn, already keep left during previous turn. + // This implies constraining the rightmost lanes in the previous turn step. + const LaneID shared_lane_delta = previous_lanes.lanes_in_turn - num_shared_lanes; + const LaneID previous_adjusted_lanes = + std::min(current_adjusted_lanes, shared_lane_delta); + const LaneID constraint_first_lane_from_the_right = + previous_lanes.first_lane_from_the_right + previous_adjusted_lanes; + + previous_lanes = {num_shared_lanes, constraint_first_lane_from_the_right}; + } + }); + }; + + std::for_each(begin(subsequent_quick_turns), end(subsequent_quick_turns), constrain_lanes); + + return steps; +} + LegGeometry resyncGeometry(LegGeometry leg_geometry, const std::vector &steps) { // The geometry uses an adjacency array-like structure for representation. diff --git a/unit_tests/util/group_by.cpp b/unit_tests/util/group_by.cpp new file mode 100644 index 000000000..d5ac08cf2 --- /dev/null +++ b/unit_tests/util/group_by.cpp @@ -0,0 +1,68 @@ +#include "util/group_by.hpp" + +#include +#include + +#include +#include + +BOOST_AUTO_TEST_SUITE(group_by_test) + +using namespace osrm; +using namespace osrm::util; + +namespace +{ + +struct Yes +{ + template bool operator()(T &&) { return true; } +}; + +struct No +{ + template bool operator()(T &&) { return false; } +}; + +struct Alternating +{ + template bool operator()(T &&) { return state = !state; } + bool state = true; +}; + +struct SubRangeCounter +{ + template void operator()(Range &&) { count += 1; } + std::size_t count = 0; +}; +} + +BOOST_AUTO_TEST_CASE(grouped_empty_test) +{ + std::vector v{}; + auto ranges = group_by(begin(v), end(v), Yes{}, SubRangeCounter{}); + BOOST_CHECK_EQUAL(ranges.count, 0); +} + +BOOST_AUTO_TEST_CASE(grouped_all_match_range_test) +{ + std::vector v{1, 2, 3}; + auto ranges = group_by(begin(v), end(v), Yes{}, SubRangeCounter{}); + BOOST_CHECK_EQUAL(ranges.count, 1); +} + +BOOST_AUTO_TEST_CASE(grouped_no_match_range_test) +{ + std::vector v{1, 2, 3}; + auto ranges = group_by(begin(v), end(v), No{}, SubRangeCounter{}); + BOOST_CHECK_EQUAL(ranges.count, 1); +} + +BOOST_AUTO_TEST_CASE(grouped_alternating_matches_range_test) +{ + std::vector v{1, 2, 3}; + auto ranges = group_by(begin(v), end(v), Alternating{}, SubRangeCounter{}); + BOOST_CHECK_EQUAL(ranges.count, v.size()); +} + +BOOST_AUTO_TEST_SUITE_END()