basic turn lane handling
This commit is contained in:
Moritz Kobitzsch
@@ -0,0 +1,505 @@
|
||||
#include "extractor/guidance/constants.hpp"
|
||||
#include "extractor/guidance/turn_discovery.hpp"
|
||||
#include "extractor/guidance/turn_lane_handler.hpp"
|
||||
#include "extractor/guidance/turn_lane_matcher.hpp"
|
||||
#include "extractor/guidance/turn_lane_augmentation.hpp"
|
||||
#include "util/simple_logger.hpp"
|
||||
#include "util/typedefs.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
#include <boost/algorithm/string/predicate.hpp>
|
||||
#include <boost/numeric/conversion/cast.hpp>
|
||||
|
||||
namespace osrm
|
||||
{
|
||||
namespace extractor
|
||||
{
|
||||
namespace guidance
|
||||
{
|
||||
namespace lanes
|
||||
{
|
||||
|
||||
namespace
|
||||
{
|
||||
std::size_t getNumberOfTurns(const Intersection &intersection)
|
||||
{
|
||||
return std::count_if(intersection.begin(), intersection.end(), [](const ConnectedRoad &road) {
|
||||
return road.entry_allowed;
|
||||
});
|
||||
}
|
||||
} // namespace
|
||||
|
||||
TurnLaneHandler::TurnLaneHandler(const util::NodeBasedDynamicGraph &node_based_graph,
|
||||
const util::NameTable &turn_lane_strings,
|
||||
const std::vector<QueryNode> &node_info_list,
|
||||
const TurnAnalysis &turn_analysis)
|
||||
: node_based_graph(node_based_graph), turn_lane_strings(turn_lane_strings),
|
||||
node_info_list(node_info_list), turn_analysis(turn_analysis)
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
Turn lanes are given in the form of strings that closely correspond to the direction modifiers
|
||||
we use for our turn types. However, we still cannot simply perform a 1:1 assignment.
|
||||
|
||||
This function parses the turn_lane_strings of a format that describes an intersection as:
|
||||
|
||||
----------
|
||||
A -^
|
||||
----------
|
||||
B -> -v
|
||||
----------
|
||||
C -v
|
||||
----------
|
||||
|
||||
with a string like |left|through;right|right| and performs an assignment onto the turns:
|
||||
for example: (130, turn slight right), (180, ramp straight), (320, turn sharp left)
|
||||
*/
|
||||
Intersection TurnLaneHandler::assignTurnLanes(const NodeID at,
|
||||
const EdgeID via_edge,
|
||||
Intersection intersection) const
|
||||
{
|
||||
// initialize to invalid
|
||||
for (auto &road : intersection)
|
||||
road.turn.instruction.lane_tupel = {0, INVALID_LANEID};
|
||||
const auto &data = node_based_graph.GetEdgeData(via_edge);
|
||||
const auto turn_lane_string = data.lane_string_id != INVALID_LANE_STRINGID
|
||||
? turn_lane_strings.GetNameForID(data.lane_string_id)
|
||||
: "";
|
||||
// going straight, due to traffic signals, we can have uncompressed geometry
|
||||
if (intersection.size() == 2 &&
|
||||
((data.lane_string_id != INVALID_LANE_STRINGID &&
|
||||
data.lane_string_id ==
|
||||
node_based_graph.GetEdgeData(intersection[1].turn.eid).lane_string_id) ||
|
||||
angularDeviation(intersection[1].turn.angle, STRAIGHT_ANGLE) < FUZZY_ANGLE_DIFFERENCE))
|
||||
return std::move(intersection);
|
||||
|
||||
auto lane_data = laneDataFromString(turn_lane_string);
|
||||
|
||||
// if we see an invalid conversion, we stop immediately
|
||||
if (!turn_lane_string.empty() && lane_data.empty())
|
||||
return std::move(intersection);
|
||||
|
||||
// might be reasonable to handle multiple turns, if we know of a sequence of lanes
|
||||
// e.g. one direction per lane, if three lanes and right, through, left available
|
||||
if (!turn_lane_string.empty() && lane_data.size() == 1 && lane_data[0].tag == "none")
|
||||
return std::move(intersection);
|
||||
|
||||
const std::size_t possible_entries = getNumberOfTurns(intersection);
|
||||
|
||||
// merge does not justify an instruction
|
||||
const bool has_merge_lane = (hasTag("merge_to_left", lane_data) ||
|
||||
hasTag("merge_to_right", lane_data));
|
||||
|
||||
// Dead end streets that don't have any left-tag. This can happen due to the fallbacks for
|
||||
// broken data/barriers.
|
||||
const bool has_non_usable_u_turn =
|
||||
(intersection[0].entry_allowed && !hasTag("none", lane_data) &&
|
||||
!hasTag("left", lane_data) &&
|
||||
!hasTag("sharp_left", lane_data) &&
|
||||
!hasTag("reverse", lane_data) &&
|
||||
lane_data.size() + 1 == possible_entries);
|
||||
|
||||
if (has_merge_lane || has_non_usable_u_turn)
|
||||
return std::move(intersection);
|
||||
|
||||
if (!lane_data.empty() && canMatchTrivially(intersection, lane_data) &&
|
||||
lane_data.size() !=
|
||||
static_cast<std::size_t>(
|
||||
lane_data.back().tag != "reverse" && intersection[0].entry_allowed ? 1 : 0) +
|
||||
possible_entries &&
|
||||
intersection[0].entry_allowed && !hasTag("none", lane_data))
|
||||
lane_data.push_back({"reverse", lane_data.back().to, lane_data.back().to});
|
||||
|
||||
bool is_simple = isSimpleIntersection(lane_data, intersection);
|
||||
|
||||
// simple intersections can be assigned directly
|
||||
if (is_simple)
|
||||
{
|
||||
lane_data = handleNoneValueAtSimpleTurn(std::move(lane_data), intersection);
|
||||
return simpleMatchTuplesToTurns(std::move(intersection), lane_data);
|
||||
}
|
||||
// if the intersection is not simple but we have lane data, we check for intersections with
|
||||
// middle islands. We have two cases. The first one is providing lane data on the current
|
||||
// segment and we only need to consider the part of the current segment. In this case we
|
||||
// partition the data and only consider the first part.
|
||||
else if (!lane_data.empty())
|
||||
{
|
||||
if (lane_data.size() >= possible_entries)
|
||||
{
|
||||
lane_data = partitionLaneData(node_based_graph.GetTarget(via_edge),
|
||||
std::move(lane_data),
|
||||
intersection)
|
||||
.first;
|
||||
|
||||
// check if we were successfull in trimming
|
||||
if (lane_data.size() == possible_entries &&
|
||||
isSimpleIntersection(lane_data, intersection))
|
||||
{
|
||||
lane_data = handleNoneValueAtSimpleTurn(std::move(lane_data), intersection);
|
||||
return simpleMatchTuplesToTurns(std::move(intersection), lane_data);
|
||||
}
|
||||
}
|
||||
}
|
||||
// The second part does not provide lane data on the current segment, but on the segment prior
|
||||
// to the turn. We try to partition the data and only consider the second part.
|
||||
else if (turn_lane_string.empty())
|
||||
{
|
||||
// acquire the lane data of a previous segment and, if possible, use it for the current
|
||||
// intersection.
|
||||
return handleTurnAtPreviousIntersection(at, via_edge, std::move(intersection));
|
||||
}
|
||||
|
||||
return std::move(intersection);
|
||||
}
|
||||
|
||||
// At segregated intersections, turn lanes will often only be specified up until the first turn. To
|
||||
// actually take the turn, we need to look back to the edge we drove onto the intersection with.
|
||||
Intersection TurnLaneHandler::handleTurnAtPreviousIntersection(const NodeID at,
|
||||
const EdgeID via_edge,
|
||||
Intersection intersection) const
|
||||
{
|
||||
NodeID previous_node = SPECIAL_NODEID;
|
||||
Intersection previous_intersection;
|
||||
EdgeID previous_id = SPECIAL_EDGEID;
|
||||
|
||||
// Get the previous lane string. We only accept strings that stem from a not-simple intersection
|
||||
// and are not empty.
|
||||
const auto previous_lane_string = [&]() -> std::string {
|
||||
if (!findPreviousIntersection(at,
|
||||
via_edge,
|
||||
intersection,
|
||||
turn_analysis,
|
||||
node_based_graph,
|
||||
previous_node,
|
||||
previous_id,
|
||||
previous_intersection))
|
||||
return "";
|
||||
|
||||
const auto &previous_data = node_based_graph.GetEdgeData(previous_id);
|
||||
auto previous_string = previous_data.lane_string_id != INVALID_LANE_STRINGID
|
||||
? turn_lane_strings.GetNameForID(previous_data.lane_string_id)
|
||||
: "";
|
||||
if (previous_string.empty())
|
||||
return "";
|
||||
|
||||
previous_intersection = turn_analysis.assignTurnTypes(
|
||||
previous_node, previous_id, std::move(previous_intersection));
|
||||
|
||||
auto previous_lane_data = laneDataFromString(previous_string);
|
||||
|
||||
if (isSimpleIntersection(previous_lane_data, previous_intersection))
|
||||
return "";
|
||||
return previous_string;
|
||||
}();
|
||||
|
||||
// no lane string, no problems
|
||||
if (previous_lane_string.empty())
|
||||
return std::move(intersection);
|
||||
|
||||
auto lane_data = laneDataFromString(previous_lane_string);
|
||||
|
||||
// stop on invalid lane data conversion
|
||||
if (lane_data.empty())
|
||||
return std::move(intersection);
|
||||
|
||||
const auto is_simple = isSimpleIntersection(lane_data, intersection);
|
||||
if (is_simple)
|
||||
{
|
||||
lane_data = handleNoneValueAtSimpleTurn(std::move(lane_data), intersection);
|
||||
return simpleMatchTuplesToTurns(std::move(intersection), lane_data);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (lane_data.size() >= getNumberOfTurns(previous_intersection) &&
|
||||
previous_intersection.size() != 2)
|
||||
{
|
||||
lane_data = partitionLaneData(node_based_graph.GetTarget(previous_id),
|
||||
std::move(lane_data),
|
||||
previous_intersection)
|
||||
.second;
|
||||
|
||||
std::sort(lane_data.begin(), lane_data.end());
|
||||
|
||||
// check if we were successfull in trimming
|
||||
if (lane_data.size() == getNumberOfTurns(intersection) &&
|
||||
isSimpleIntersection(lane_data, intersection))
|
||||
{
|
||||
lane_data = handleNoneValueAtSimpleTurn(std::move(lane_data), intersection);
|
||||
return simpleMatchTuplesToTurns(std::move(intersection), lane_data);
|
||||
}
|
||||
}
|
||||
}
|
||||
return std::move(intersection);
|
||||
}
|
||||
|
||||
|
||||
/* A simple intersection does not depend on the next intersection coming up. This is important
|
||||
* for turn lanes, since traffic signals and/or segregated a intersection can influence the
|
||||
* interpretation of turn-lanes at a given turn.
|
||||
*
|
||||
* Here we check for a simple intersection. A simple intersection has a long enough segment
|
||||
* followin the turn, offers no straight turn, or only non-trivial turn operations.
|
||||
*/
|
||||
bool TurnLaneHandler::isSimpleIntersection(const LaneDataVector &lane_data,
|
||||
const Intersection &intersection) const
|
||||
{
|
||||
if (lane_data.empty())
|
||||
return false;
|
||||
// if we are on a straight road, turn lanes are only reasonable in connection to the next
|
||||
// intersection, or in case of a merge. If not all but one (straight) are merges, we don't
|
||||
// consider the intersection simple
|
||||
if (intersection.size() == 2)
|
||||
{
|
||||
return std::count_if(
|
||||
lane_data.begin(),
|
||||
lane_data.end(),
|
||||
[](const TurnLaneData &data) { return boost::starts_with(data.tag, "merge"); }) +
|
||||
std::size_t{1} >=
|
||||
lane_data.size();
|
||||
}
|
||||
|
||||
// in case an intersection offers far more lane data items than actual turns, some of them
|
||||
// have
|
||||
// to be for another intersection. A single additional item can be for an invalid bus lane.
|
||||
const auto num_turns = [&]() {
|
||||
auto count = getNumberOfTurns(intersection);
|
||||
if (count < lane_data.size() && !intersection[0].entry_allowed &&
|
||||
lane_data.back().tag == "reverse")
|
||||
return count + 1;
|
||||
return count;
|
||||
}();
|
||||
|
||||
// more than two additional lane data entries -> lanes target a different intersection
|
||||
if (num_turns + std::size_t{2} <= lane_data.size())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// single additional lane data entry is alright, if it is none at the side. This usually
|
||||
// refers to a bus-lane
|
||||
if (num_turns + std::size_t{1} == lane_data.size() && lane_data.front().tag != "none" &&
|
||||
lane_data.back().tag != "none")
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// more turns than lane data
|
||||
if (num_turns > lane_data.size() &&
|
||||
lane_data.end() ==
|
||||
std::find_if(lane_data.begin(), lane_data.end(), [](const TurnLaneData &data) {
|
||||
return data.tag == "none";
|
||||
}))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (num_turns > lane_data.size() && intersection[0].entry_allowed &&
|
||||
!( hasTag("reverse", lane_data) ||
|
||||
(lane_data.back().tag != "left" && lane_data.back().tag != "sharp_left")))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// check if we can find a valid 1:1 mapping in a straightforward manner
|
||||
bool all_simple = true;
|
||||
bool has_none = false;
|
||||
std::unordered_set<std::size_t> matched_indices;
|
||||
for (const auto &data : lane_data)
|
||||
{
|
||||
if (data.tag == "none")
|
||||
{
|
||||
has_none = true;
|
||||
continue;
|
||||
}
|
||||
|
||||
const auto best_match = [&]() {
|
||||
if (data.tag != "reverse" || lane_data.size() == 1)
|
||||
return findBestMatch(data.tag, intersection);
|
||||
|
||||
// lane_data.size() > 1
|
||||
if (lane_data.back().tag == "reverse")
|
||||
return findBestMatchForReverse(lane_data[lane_data.size() - 2].tag, intersection);
|
||||
|
||||
BOOST_ASSERT(lane_data.front().tag == "reverse");
|
||||
return findBestMatchForReverse(lane_data[1].tag, intersection);
|
||||
}();
|
||||
std::size_t match_index = std::distance(intersection.begin(), best_match);
|
||||
all_simple &= (matched_indices.count(match_index) == 0);
|
||||
matched_indices.insert(match_index);
|
||||
// in case of u-turns, we might need to activate them first
|
||||
all_simple &= (best_match->entry_allowed || match_index == 0);
|
||||
all_simple &= isValidMatch(data.tag, best_match->turn.instruction);
|
||||
}
|
||||
|
||||
// either all indices are matched, or we have a single none-value
|
||||
if (all_simple && (matched_indices.size() == lane_data.size() ||
|
||||
(matched_indices.size() + 1 == lane_data.size() && has_none)))
|
||||
return true;
|
||||
|
||||
// better save than sorry
|
||||
return false;
|
||||
}
|
||||
|
||||
std::pair<LaneDataVector, LaneDataVector> TurnLaneHandler::partitionLaneData(
|
||||
const NodeID at, LaneDataVector turn_lane_data, const Intersection &intersection) const
|
||||
{
|
||||
BOOST_ASSERT(turn_lane_data.size() >= getNumberOfTurns(intersection));
|
||||
/*
|
||||
* A Segregated intersection can provide turn lanes for turns that are not yet possible.
|
||||
* The straightforward example would be coming up to the following situation:
|
||||
* (1) (2)
|
||||
* | A | | A |
|
||||
* | | | | ^ |
|
||||
* | v | | | |
|
||||
* ------- ----------- ------
|
||||
* B ->-^ B
|
||||
* ------- ----------- ------
|
||||
* B ->-v B
|
||||
* ------- ----------- ------
|
||||
* | A | | A |
|
||||
*
|
||||
* Traveling on road B, we have to pass A at (1) to turn left onto A at (2). The turn
|
||||
* lane itself may only be specified prior to (1) and/or could be repeated between (1)
|
||||
* and (2). To make sure to announce the lane correctly, we need to treat the (in this
|
||||
* case left) turn lane as if it were to continue straight onto the intersection and
|
||||
* look back between (1) and (2) to make sure we find the correct lane for the left-turn.
|
||||
*
|
||||
* Intersections like these have two parts. Turns that can be made at the first intersection and
|
||||
* turns that have to be made at the second. The partitioning returns the lane data split into
|
||||
* two parts, one for the first and one for the second intersection.
|
||||
*/
|
||||
|
||||
// Try and maitch lanes to available turns. For Turns that are not directly matchable, check
|
||||
// whether we can match them at the upcoming intersection.
|
||||
|
||||
const auto straightmost = findClosestTurn(intersection, STRAIGHT_ANGLE);
|
||||
|
||||
BOOST_ASSERT(straightmost < intersection.cend());
|
||||
|
||||
// we need to be able to enter the straightmost turn
|
||||
if (!straightmost->entry_allowed)
|
||||
return {turn_lane_data, {}};
|
||||
|
||||
std::vector<bool> matched_at_first(turn_lane_data.size(), false);
|
||||
std::vector<bool> matched_at_second(turn_lane_data.size(), false);
|
||||
|
||||
// find out about the next intersection. To check for valid matches, we also need the turn types
|
||||
auto next_intersection = turn_analysis.getIntersection(at, straightmost->turn.eid);
|
||||
next_intersection =
|
||||
turn_analysis.assignTurnTypes(at, straightmost->turn.eid, std::move(next_intersection));
|
||||
|
||||
// check where we can match turn lanes
|
||||
std::size_t straightmost_tag_index = turn_lane_data.size();
|
||||
for (std::size_t lane = 0; lane < turn_lane_data.size(); ++lane)
|
||||
{
|
||||
if (turn_lane_data[lane].tag == "none" || turn_lane_data[lane].tag == "reverse")
|
||||
continue;
|
||||
|
||||
const auto best_match = findBestMatch(turn_lane_data[lane].tag, intersection);
|
||||
if (isValidMatch(turn_lane_data[lane].tag, best_match->turn.instruction))
|
||||
{
|
||||
matched_at_first[lane] = true;
|
||||
|
||||
if (straightmost == best_match)
|
||||
straightmost_tag_index = lane;
|
||||
}
|
||||
|
||||
const auto best_match_at_next_intersection =
|
||||
findBestMatch(turn_lane_data[lane].tag, next_intersection);
|
||||
if (isValidMatch(turn_lane_data[lane].tag,
|
||||
best_match_at_next_intersection->turn.instruction))
|
||||
matched_at_second[lane] = true;
|
||||
|
||||
// we need to match all items to either the current or the next intersection
|
||||
if (!(matched_at_first[lane] || matched_at_second[lane]))
|
||||
return {turn_lane_data, {}};
|
||||
}
|
||||
|
||||
std::size_t none_index = std::distance(turn_lane_data.begin(),findTag("none", turn_lane_data));
|
||||
|
||||
// if the turn lanes are pull forward, we might have to add an additional straight tag
|
||||
// did we find something that matches against the straightmost road?
|
||||
if (straightmost_tag_index == turn_lane_data.size())
|
||||
{
|
||||
if (none_index != turn_lane_data.size())
|
||||
straightmost_tag_index = none_index;
|
||||
}
|
||||
|
||||
// TODO handle reverse
|
||||
|
||||
// handle none values
|
||||
if (none_index != turn_lane_data.size())
|
||||
{
|
||||
if (static_cast<std::size_t>(
|
||||
std::count(matched_at_first.begin(), matched_at_first.end(), true)) <=
|
||||
getNumberOfTurns(intersection))
|
||||
matched_at_first[none_index] = true;
|
||||
|
||||
if (static_cast<std::size_t>(
|
||||
std::count(matched_at_second.begin(), matched_at_second.end(), true)) <=
|
||||
getNumberOfTurns(next_intersection))
|
||||
matched_at_second[none_index] = true;
|
||||
}
|
||||
|
||||
const auto augmentEntry = [&](TurnLaneData &data) {
|
||||
for (std::size_t lane = 0; lane < turn_lane_data.size(); ++lane)
|
||||
if (matched_at_second[lane])
|
||||
{
|
||||
data.from = std::min(turn_lane_data[lane].from, data.from);
|
||||
data.to = std::max(turn_lane_data[lane].to, data.to);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
LaneDataVector first, second;
|
||||
for (std::size_t lane = 0; lane < turn_lane_data.size(); ++lane)
|
||||
{
|
||||
|
||||
if (matched_at_second[lane])
|
||||
second.push_back(turn_lane_data[lane]);
|
||||
|
||||
// augment straightmost at this intersection to match all turns that happen at the next
|
||||
if (lane == straightmost_tag_index)
|
||||
{
|
||||
augmentEntry(turn_lane_data[straightmost_tag_index]);
|
||||
}
|
||||
|
||||
if (matched_at_first[lane])
|
||||
first.push_back(turn_lane_data[lane]);
|
||||
}
|
||||
|
||||
if (straightmost_tag_index == turn_lane_data.size() &&
|
||||
static_cast<std::size_t>(
|
||||
std::count(matched_at_second.begin(), matched_at_second.end(), true)) ==
|
||||
getNumberOfTurns(next_intersection))
|
||||
{
|
||||
TurnLaneData data = {"through", 255, 0};
|
||||
augmentEntry(data);
|
||||
first.push_back(data);
|
||||
std::sort(first.begin(), first.end());
|
||||
}
|
||||
|
||||
// TODO augment straightmost turn
|
||||
return {std::move(first), std::move(second)};
|
||||
}
|
||||
|
||||
Intersection TurnLaneHandler::simpleMatchTuplesToTurns(Intersection intersection,
|
||||
const LaneDataVector &lane_data) const
|
||||
{
|
||||
if (lane_data.empty() || !canMatchTrivially(intersection, lane_data))
|
||||
return std::move(intersection);
|
||||
|
||||
BOOST_ASSERT(!hasTag("none", lane_data));
|
||||
BOOST_ASSERT(std::count_if(lane_data.begin(), lane_data.end(), [](const TurnLaneData &data) {
|
||||
return boost::starts_with(data.tag, "merge");
|
||||
}) == 0);
|
||||
|
||||
return triviallyMatchLanesToTurns(std::move(intersection), lane_data, node_based_graph);
|
||||
}
|
||||
|
||||
} // namespace lanes
|
||||
} // namespace guidance
|
||||
} // namespace extractor
|
||||
} // namespace osrm
|
||||
Reference in New Issue
Block a user