refactor of turn analysis into turn handlers
This commit is contained in:
Moritz Kobitzsch
committed by
Patrick Niklaus
Patrick Niklaus
parent
dfd180a292
commit
fcbf527ba5
@@ -0,0 +1,257 @@
|
||||
#include "extractor/guidance/constants.hpp"
|
||||
#include "extractor/guidance/roundabout_handler.hpp"
|
||||
#include "extractor/guidance/toolkit.hpp"
|
||||
|
||||
#include "util/simple_logger.hpp"
|
||||
|
||||
#include <cmath>
|
||||
#include <set>
|
||||
#include <unordered_set>
|
||||
|
||||
#include <boost/assert.hpp>
|
||||
|
||||
namespace osrm
|
||||
{
|
||||
namespace extractor
|
||||
{
|
||||
namespace guidance
|
||||
{
|
||||
|
||||
RoundaboutHandler::RoundaboutHandler(const util::NodeBasedDynamicGraph &node_based_graph,
|
||||
const std::vector<QueryNode> &node_info_list,
|
||||
const util::NameTable &name_table)
|
||||
: IntersectionHandler(node_based_graph, node_info_list, name_table)
|
||||
{
|
||||
}
|
||||
|
||||
RoundaboutHandler::~RoundaboutHandler() {}
|
||||
|
||||
bool RoundaboutHandler::canProcess(const NodeID from_nid,
|
||||
const EdgeID via_eid,
|
||||
const Intersection &intersection) const
|
||||
{
|
||||
const auto flags = getRoundaboutFlags(from_nid, via_eid, intersection);
|
||||
return flags.on_roundabout || flags.can_enter;
|
||||
}
|
||||
|
||||
Intersection RoundaboutHandler::
|
||||
operator()(const NodeID from_nid, const EdgeID via_eid, Intersection intersection) const
|
||||
{
|
||||
const auto flags = getRoundaboutFlags(from_nid, via_eid, intersection);
|
||||
const bool is_rotary = isRotary(node_based_graph.GetTarget(via_eid));
|
||||
// find the radius of the roundabout
|
||||
return handleRoundabouts(is_rotary, via_eid, flags.on_roundabout, flags.can_exit_separately,
|
||||
std::move(intersection));
|
||||
}
|
||||
|
||||
detail::RoundaboutFlags RoundaboutHandler::getRoundaboutFlags(
|
||||
const NodeID from_nid, const EdgeID via_eid, const Intersection &intersection) const
|
||||
{
|
||||
const auto &in_edge_data = node_based_graph.GetEdgeData(via_eid);
|
||||
bool on_roundabout = in_edge_data.roundabout;
|
||||
bool can_enter_roundabout = false;
|
||||
bool can_exit_roundabout_separately = false;
|
||||
for (const auto &road : intersection)
|
||||
{
|
||||
const auto &edge_data = node_based_graph.GetEdgeData(road.turn.eid);
|
||||
// only check actual outgoing edges
|
||||
if (edge_data.reversed)
|
||||
continue;
|
||||
|
||||
if (edge_data.roundabout)
|
||||
{
|
||||
can_enter_roundabout = true;
|
||||
}
|
||||
// Exiting roundabouts at an entry point is technically a data-modelling issue.
|
||||
// This workaround handles cases in which an exit follows the entry.
|
||||
// To correctly represent perceived exits, we only count exits leading to a
|
||||
// separate vertex than the one we are coming from that are in the direction of
|
||||
// the roundabout.
|
||||
// The sorting of the angles represents a problem for left-sided driving, though.
|
||||
// FIXME in case of left-sided driving, we have to check whether we can enter the
|
||||
// roundabout later in the cycle, rather than prior.
|
||||
// FIXME requires consideration of crossing the roundabout
|
||||
else if (node_based_graph.GetTarget(road.turn.eid) != from_nid && !can_enter_roundabout)
|
||||
{
|
||||
can_exit_roundabout_separately = true;
|
||||
}
|
||||
}
|
||||
return {on_roundabout, can_enter_roundabout, can_exit_roundabout_separately};
|
||||
}
|
||||
|
||||
bool RoundaboutHandler::isRotary(const NodeID nid) const
|
||||
{
|
||||
// translate a node ID into its respective coordinate stored in the node_info_list
|
||||
const auto getCoordinate = [this](const NodeID node) {
|
||||
return util::Coordinate(node_info_list[node].lon, node_info_list[node].lat);
|
||||
};
|
||||
|
||||
unsigned roundabout_name_id = 0;
|
||||
std::unordered_set<unsigned> connected_names;
|
||||
|
||||
const auto getNextOnRoundabout = [this, &roundabout_name_id,
|
||||
&connected_names](const NodeID node) {
|
||||
EdgeID continue_edge = SPECIAL_EDGEID;
|
||||
for (const auto edge : node_based_graph.GetAdjacentEdgeRange(node))
|
||||
{
|
||||
const auto &edge_data = node_based_graph.GetEdgeData(edge);
|
||||
if (!edge_data.reversed && edge_data.roundabout)
|
||||
{
|
||||
if (SPECIAL_EDGEID != continue_edge)
|
||||
{
|
||||
// fork in roundabout
|
||||
return SPECIAL_EDGEID;
|
||||
}
|
||||
// roundabout does not keep its name
|
||||
if (roundabout_name_id != 0 && roundabout_name_id != edge_data.name_id &&
|
||||
requiresNameAnnounced(name_table.GetNameForID(roundabout_name_id),
|
||||
name_table.GetNameForID(edge_data.name_id)))
|
||||
{
|
||||
return SPECIAL_EDGEID;
|
||||
}
|
||||
|
||||
roundabout_name_id = edge_data.name_id;
|
||||
|
||||
continue_edge = edge;
|
||||
}
|
||||
else if (!edge_data.roundabout)
|
||||
{
|
||||
// remember all connected road names
|
||||
connected_names.insert(edge_data.name_id);
|
||||
}
|
||||
}
|
||||
return continue_edge;
|
||||
};
|
||||
// the roundabout radius has to be the same for all locations we look at it from
|
||||
// to guarantee this, we search the full roundabout for its vertices
|
||||
// and select the three smalles ids
|
||||
std::set<NodeID> roundabout_nodes; // needs to be sorted
|
||||
|
||||
// this value is a hard abort to deal with potential self-loops
|
||||
NodeID last_node = nid;
|
||||
while (0 == roundabout_nodes.count(last_node))
|
||||
{
|
||||
roundabout_nodes.insert(last_node);
|
||||
const auto eid = getNextOnRoundabout(last_node);
|
||||
|
||||
if (eid == SPECIAL_EDGEID)
|
||||
{
|
||||
util::SimpleLogger().Write(logDEBUG) << "Non-Loop Roundabout found.";
|
||||
return false;
|
||||
}
|
||||
|
||||
last_node = node_based_graph.GetTarget(eid);
|
||||
}
|
||||
|
||||
// do we have a dedicated name for the rotary, if not its a roundabout
|
||||
// This function can theoretically fail if the roundabout name is partly
|
||||
// used with a reference and without. This will be fixed automatically
|
||||
// when we handle references separately or if the useage is more consistent
|
||||
if (roundabout_name_id == 0 || connected_names.count(roundabout_name_id))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (roundabout_nodes.size() <= 1)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
// calculate the radius of the roundabout/rotary. For two coordinates, we assume a minimal
|
||||
// circle
|
||||
// with both vertices right at the other side (so half their distance in meters).
|
||||
// Otherwise, we construct a circle through the first tree vertices.
|
||||
const auto getRadius = [&roundabout_nodes, &getCoordinate]() {
|
||||
auto node_itr = roundabout_nodes.begin();
|
||||
if (roundabout_nodes.size() == 2)
|
||||
{
|
||||
const auto first = getCoordinate(*node_itr++), second = getCoordinate(*node_itr++);
|
||||
return 0.5 * util::coordinate_calculation::haversineDistance(first, second);
|
||||
}
|
||||
else
|
||||
{
|
||||
const auto first = getCoordinate(*node_itr++), second = getCoordinate(*node_itr++),
|
||||
third = getCoordinate(*node_itr++);
|
||||
return util::coordinate_calculation::circleRadius(first, second, third);
|
||||
}
|
||||
};
|
||||
const double radius = getRadius();
|
||||
|
||||
// check whether the circle computation has gone wrong
|
||||
// The radius computation can result in infinity, if the three coordinates are non-distinct.
|
||||
// To stay on the safe side, we say its not a rotary
|
||||
if (std::isinf(radius))
|
||||
return false;
|
||||
|
||||
return radius > MAX_ROUNDABOUT_RADIUS;
|
||||
}
|
||||
|
||||
Intersection RoundaboutHandler::handleRoundabouts(const bool is_rotary,
|
||||
const EdgeID via_eid,
|
||||
const bool on_roundabout,
|
||||
const bool can_exit_roundabout_separately,
|
||||
Intersection intersection) const
|
||||
{
|
||||
// TODO requires differentiation between roundabouts and rotaries
|
||||
// detect via radius (get via circle through three vertices)
|
||||
NodeID node_v = node_based_graph.GetTarget(via_eid);
|
||||
if (on_roundabout)
|
||||
{
|
||||
// Shoule hopefully have only a single exit and continue
|
||||
// at least for cars. How about bikes?
|
||||
for (auto &road : intersection)
|
||||
{
|
||||
auto &turn = road.turn;
|
||||
const auto &out_data = node_based_graph.GetEdgeData(road.turn.eid);
|
||||
if (out_data.roundabout)
|
||||
{
|
||||
// TODO can forks happen in roundabouts? E.g. required lane changes
|
||||
if (1 == node_based_graph.GetDirectedOutDegree(node_v))
|
||||
{
|
||||
// No turn possible.
|
||||
turn.instruction = TurnInstruction::NO_TURN();
|
||||
}
|
||||
else
|
||||
{
|
||||
turn.instruction =
|
||||
TurnInstruction::REMAIN_ROUNDABOUT(is_rotary, getTurnDirection(turn.angle));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
turn.instruction =
|
||||
TurnInstruction::EXIT_ROUNDABOUT(is_rotary, getTurnDirection(turn.angle));
|
||||
}
|
||||
}
|
||||
return intersection;
|
||||
}
|
||||
else
|
||||
for (auto &road : intersection)
|
||||
{
|
||||
if (!road.entry_allowed)
|
||||
continue;
|
||||
auto &turn = road.turn;
|
||||
const auto &out_data = node_based_graph.GetEdgeData(turn.eid);
|
||||
if (out_data.roundabout)
|
||||
{
|
||||
turn.instruction =
|
||||
TurnInstruction::ENTER_ROUNDABOUT(is_rotary, getTurnDirection(turn.angle));
|
||||
if (can_exit_roundabout_separately)
|
||||
{
|
||||
if (turn.instruction.type == TurnType::EnterRotary)
|
||||
turn.instruction.type = TurnType::EnterRotaryAtExit;
|
||||
if (turn.instruction.type == TurnType::EnterRoundabout)
|
||||
turn.instruction.type = TurnType::EnterRoundaboutAtExit;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
turn.instruction = TurnInstruction::ENTER_AND_EXIT_ROUNDABOUT(
|
||||
is_rotary, getTurnDirection(turn.angle));
|
||||
}
|
||||
}
|
||||
return intersection;
|
||||
}
|
||||
|
||||
} // namespace guidance
|
||||
} // namespace extractor
|
||||
} // namespace osrm
|
||||
Reference in New Issue
Block a user