basic turn lane handling
This commit is contained in:
Moritz Kobitzsch
@@ -0,0 +1,304 @@
|
||||
#include "extractor/guidance/turn_lane_augmentation.hpp"
|
||||
#include "util/simple_logger.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstddef>
|
||||
#include <utility>
|
||||
#include <boost/assert.hpp>
|
||||
|
||||
namespace osrm
|
||||
{
|
||||
namespace extractor
|
||||
{
|
||||
namespace guidance
|
||||
{
|
||||
namespace lanes
|
||||
{
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
const constexpr char *tag_by_modifier[] = {"reverse",
|
||||
"sharp_right",
|
||||
"right",
|
||||
"slight_right",
|
||||
"through",
|
||||
"slight_left",
|
||||
"left",
|
||||
"sharp_left"};
|
||||
|
||||
std::size_t getNumberOfTurns(const Intersection &intersection)
|
||||
{
|
||||
return std::count_if(intersection.begin(), intersection.end(), [](const ConnectedRoad &road) {
|
||||
return road.entry_allowed;
|
||||
});
|
||||
}
|
||||
|
||||
LaneDataVector augmentMultiple(const std::size_t none_index,
|
||||
const std::size_t connection_count,
|
||||
LaneDataVector lane_data,
|
||||
const Intersection &intersection)
|
||||
{
|
||||
|
||||
// a none-turn is allowing multiple turns. we have to add a lane-data entry for
|
||||
// every possible turn. This should, hopefully, only be the case for single lane
|
||||
// entries?
|
||||
|
||||
// looking at the left side first
|
||||
const auto range = [&]() {
|
||||
if (none_index == 0)
|
||||
{
|
||||
// find first connection_count - lane_data.size() valid turns
|
||||
std::size_t count = 0;
|
||||
for (std::size_t intersection_index = 1; intersection_index < intersection.size();
|
||||
++intersection_index)
|
||||
{
|
||||
|
||||
count += static_cast<int>(intersection[intersection_index].entry_allowed);
|
||||
if (count > connection_count - lane_data.size())
|
||||
return std::make_pair(std::size_t{1}, intersection_index + 1);
|
||||
}
|
||||
}
|
||||
else if (none_index + 1 == lane_data.size())
|
||||
{
|
||||
BOOST_ASSERT(!lane_data.empty());
|
||||
// find last connection-count - last_data.size() valid turns
|
||||
std::size_t count = 0;
|
||||
for (std::size_t intersection_index = intersection.size() - 1; intersection_index > 0;
|
||||
--intersection_index)
|
||||
{
|
||||
count += static_cast<int>(intersection[intersection_index].entry_allowed);
|
||||
if (count > connection_count - lane_data.size())
|
||||
return std::make_pair(intersection_index, intersection.size());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// skip the first #index valid turns, find next connection_count -
|
||||
// lane_data.size() valid ones
|
||||
|
||||
std::size_t begin = 1, count = 0, intersection_index;
|
||||
for (intersection_index = 1; intersection_index < intersection.size();
|
||||
++intersection_index)
|
||||
{
|
||||
count += static_cast<int>(intersection[intersection_index].entry_allowed);
|
||||
// if we reach the amount of
|
||||
if (count >= none_index)
|
||||
{
|
||||
begin = intersection_index + 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// reset count to find the number of necessary entries
|
||||
count = 0;
|
||||
for (intersection_index = begin; intersection_index < intersection.size();
|
||||
++intersection_index)
|
||||
{
|
||||
count += static_cast<int>(intersection[intersection_index].entry_allowed);
|
||||
if (count > connection_count - lane_data.size())
|
||||
{
|
||||
return std::make_pair(begin, intersection_index + 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
// this should, theoretically, never be reached
|
||||
util::SimpleLogger().Write(logWARNING) << "Failed lane assignment. Reached bad situation.";
|
||||
return std::make_pair(std::size_t{0}, std::size_t{0});
|
||||
}();
|
||||
for (auto intersection_index = range.first; intersection_index < range.second;
|
||||
++intersection_index)
|
||||
{
|
||||
if (intersection[intersection_index].entry_allowed)
|
||||
{
|
||||
// FIXME this probably can be only a subset of these turns here?
|
||||
lane_data.push_back({tag_by_modifier[intersection[intersection_index]
|
||||
.turn.instruction.direction_modifier],
|
||||
lane_data[none_index].from,
|
||||
lane_data[none_index].to});
|
||||
}
|
||||
}
|
||||
lane_data.erase(lane_data.begin() + none_index);
|
||||
return std::move(lane_data);
|
||||
}
|
||||
|
||||
// Merging none-tag into its neighboring fields
|
||||
// This handles situations like "left | | | right".
|
||||
LaneDataVector mergeNoneTag(const std::size_t none_index, LaneDataVector lane_data)
|
||||
{
|
||||
|
||||
if (none_index == 0 || none_index + 1 == lane_data.size())
|
||||
{
|
||||
if (none_index == 0)
|
||||
{
|
||||
lane_data[1].from = lane_data[0].from;
|
||||
}
|
||||
else
|
||||
{
|
||||
lane_data[none_index - 1].to = lane_data[none_index].to;
|
||||
}
|
||||
lane_data.erase(lane_data.begin() + none_index);
|
||||
}
|
||||
else if (lane_data[none_index].to - lane_data[none_index].from <= 1)
|
||||
{
|
||||
lane_data[none_index - 1].to = lane_data[none_index].from;
|
||||
lane_data[none_index + 1].from = lane_data[none_index].to;
|
||||
|
||||
lane_data.erase(lane_data.begin() + none_index);
|
||||
}
|
||||
return std::move(lane_data);
|
||||
}
|
||||
|
||||
LaneDataVector handleRenamingSituations(const std::size_t none_index,
|
||||
LaneDataVector lane_data,
|
||||
const Intersection &intersection)
|
||||
{
|
||||
bool has_right = false;
|
||||
bool has_through = false;
|
||||
bool has_left = false;
|
||||
for (const auto &road : intersection)
|
||||
{
|
||||
if (!road.entry_allowed)
|
||||
continue;
|
||||
|
||||
const auto modifier = road.turn.instruction.direction_modifier;
|
||||
has_right |= modifier == DirectionModifier::Right;
|
||||
has_right |= modifier == DirectionModifier::SlightRight;
|
||||
has_right |= modifier == DirectionModifier::SharpRight;
|
||||
has_through |= modifier == DirectionModifier::Straight;
|
||||
has_left |= modifier == DirectionModifier::Left;
|
||||
has_left |= modifier == DirectionModifier::SlightLeft;
|
||||
has_left |= modifier == DirectionModifier::SharpLeft;
|
||||
}
|
||||
|
||||
|
||||
// find missing tag and augment neighboring, if possible
|
||||
if (none_index == 0)
|
||||
{
|
||||
if (has_right &&
|
||||
(lane_data.size() == 1 || (lane_data[none_index + 1].tag != "sharp_right" &&
|
||||
lane_data[none_index + 1].tag != "right")))
|
||||
{
|
||||
lane_data[none_index].tag = "right";
|
||||
if (lane_data.size() > 1 && lane_data[none_index + 1].tag == "through")
|
||||
{
|
||||
lane_data[none_index + 1].from = lane_data[none_index].from;
|
||||
// turning right through a possible through lane is not possible
|
||||
lane_data[none_index].to = lane_data[none_index].from;
|
||||
}
|
||||
}
|
||||
else if (has_through &&
|
||||
(lane_data.size() == 1 || lane_data[none_index + 1].tag != "through"))
|
||||
{
|
||||
lane_data[none_index].tag = "through";
|
||||
}
|
||||
}
|
||||
else if (none_index + 1 == lane_data.size())
|
||||
{
|
||||
if (has_left && ((lane_data[none_index - 1].tag != "sharp_left" &&
|
||||
lane_data[none_index - 1].tag != "left")))
|
||||
{
|
||||
lane_data[none_index].tag = "left";
|
||||
if (lane_data[none_index - 1].tag == "through")
|
||||
{
|
||||
lane_data[none_index - 1].to = lane_data[none_index].to;
|
||||
// turning left through a possible through lane is not possible
|
||||
lane_data[none_index].from = lane_data[none_index].to;
|
||||
}
|
||||
}
|
||||
else if (has_through && lane_data[none_index - 1].tag != "through")
|
||||
{
|
||||
lane_data[none_index].tag = "through";
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if ((lane_data[none_index + 1].tag == "left" ||
|
||||
lane_data[none_index + 1].tag == "slight_left" ||
|
||||
lane_data[none_index + 1].tag == "sharp_left") &&
|
||||
(lane_data[none_index - 1].tag == "right" ||
|
||||
lane_data[none_index - 1].tag == "slight_right" ||
|
||||
lane_data[none_index - 1].tag == "sharp_right"))
|
||||
{
|
||||
lane_data[none_index].tag = "through";
|
||||
}
|
||||
}
|
||||
return std::move(lane_data);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
/*
|
||||
Lanes can have the tag none. While a nice feature for visibility, it is a terrible feature
|
||||
for parsing. None can be part of neighboring turns, or not. We have to look at both the
|
||||
intersection and the lane data to see what turns we have to augment by the none-lanes
|
||||
*/
|
||||
LaneDataVector handleNoneValueAtSimpleTurn(LaneDataVector lane_data,
|
||||
const Intersection &intersection)
|
||||
{
|
||||
const bool needs_no_processing =
|
||||
(intersection.empty() || lane_data.empty() || !hasTag("none", lane_data));
|
||||
|
||||
if (needs_no_processing)
|
||||
return std::move(lane_data);
|
||||
|
||||
// FIXME all this needs to consider the number of lanes at the target to ensure that we
|
||||
// augment lanes correctly, if the target lane allows for more turns
|
||||
//
|
||||
// -----------------
|
||||
//
|
||||
// ----- ----
|
||||
// -v |
|
||||
// ----- |
|
||||
// | | |
|
||||
//
|
||||
// A situation like this would allow a right turn from the through lane.
|
||||
//
|
||||
// -----------------
|
||||
//
|
||||
// ----- --------
|
||||
// -v |
|
||||
// ----- |
|
||||
// | |
|
||||
//
|
||||
// Here, the number of lanes in the right road would not allow turns from both lanes, but
|
||||
// only from the right one.
|
||||
|
||||
const std::size_t connection_count =
|
||||
getNumberOfTurns(intersection) -
|
||||
((intersection[0].entry_allowed && lane_data.back().tag != "reverse") ? 1 : 0);
|
||||
|
||||
// TODO check for impossible turns to see whether the turn lane is at the correct place
|
||||
const std::size_t none_index = std::distance(lane_data.begin(), findTag("none", lane_data));
|
||||
BOOST_ASSERT(none_index != lane_data.size());
|
||||
// we have to create multiple turns
|
||||
if (connection_count > lane_data.size())
|
||||
{
|
||||
lane_data =
|
||||
augmentMultiple(none_index, connection_count, std::move(lane_data), intersection);
|
||||
}
|
||||
// we have to reduce it, assigning it to neighboring turns
|
||||
else if (connection_count < lane_data.size())
|
||||
{
|
||||
// a prerequisite is simple turns. Larger differences should not end up here
|
||||
// an additional line at the side is only reasonable if it is targeting public
|
||||
// service vehicles. Otherwise, we should not have it
|
||||
BOOST_ASSERT(connection_count + 1 == lane_data.size());
|
||||
|
||||
lane_data = mergeNoneTag(none_index, std::move(lane_data));
|
||||
}
|
||||
// we have to rename and possibly augment existing ones. The pure count remains the
|
||||
// same.
|
||||
else
|
||||
{
|
||||
lane_data = handleRenamingSituations(none_index, std::move(lane_data), intersection);
|
||||
}
|
||||
// finally make sure we are still sorted
|
||||
std::sort(lane_data.begin(), lane_data.end());
|
||||
return std::move(lane_data);
|
||||
}
|
||||
|
||||
} // namespace lanes
|
||||
} // namespace guidance
|
||||
} // namespace extractor
|
||||
} // namespace osrm
|
||||
Reference in New Issue
Block a user