Fix NoRoute for waypoints with low reachability edges #2914
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -80,6 +80,42 @@ void via_discontinuity( | |
| } | ||
| } | ||
|
|
||
| inline bool is_through_point(const valhalla::Location& l) { | ||
| return l.type() == valhalla::Location::kThrough || l.type() == valhalla::Location::kBreakThrough; | ||
| } | ||
|
|
||
| inline bool is_break_point(const valhalla::Location& l) { | ||
| return l.type() == valhalla::Location::kBreak || l.type() == valhalla::Location::kBreakThrough; | ||
| } | ||
|
|
||
| inline bool is_high_reachable(const valhalla::Location& loc, | ||
kevinkreiser marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| const valhalla::Location_PathEdge& edge) { | ||
| return edge.inbound_reach() >= loc.minimum_reachability() && | ||
| edge.outbound_reach() >= loc.minimum_reachability(); | ||
| } | ||
|
|
||
| inline void remove_edges_with_low_reachability(valhalla::Location& loc) { | ||
| auto new_end = std::remove_if(loc.mutable_path_edges()->begin(), loc.mutable_path_edges()->end(), | ||
| [&loc](const auto& pe) { return !is_high_reachable(loc, pe); }); | ||
| int start_idx = std::distance(loc.mutable_path_edges()->begin(), new_end); | ||
| loc.mutable_path_edges()->DeleteSubrange(start_idx, loc.path_edges_size() - start_idx); | ||
|
|
||
| new_end = std::remove_if(loc.mutable_filtered_edges()->begin(), loc.mutable_filtered_edges()->end(), | ||
| [&loc](const auto& pe) { return !is_high_reachable(loc, pe); }); | ||
| start_idx = std::distance(loc.mutable_filtered_edges()->begin(), new_end); | ||
| loc.mutable_filtered_edges()->DeleteSubrange(start_idx, loc.filtered_edges_size() - start_idx); | ||
| } | ||
|
|
||
| inline void remove_all_edges_except_one(valhalla::Location& loc, const GraphId& edgeid) { | ||
kevinkreiser marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| while (loc.path_edges_size() > 1) { | ||
| if (loc.path_edges().rbegin()->graph_id() == edgeid) { | ||
| loc.mutable_path_edges()->SwapElements(0, loc.path_edges_size() - 1); | ||
| } | ||
| loc.mutable_path_edges()->RemoveLast(); | ||
| } | ||
| loc.mutable_filtered_edges()->Clear(); | ||
| } | ||
|
|
||
| /** | ||
| // removes any edges from the location that aren't connected to it (because of radius) | ||
| void remove_edges(const GraphId& edge_id, valhalla::Location& loc, GraphReader& reader) { | ||
|
There was a problem hiding this comment. delete this function i mean, since you are doing it a different way |
||
|
|
@@ -382,10 +418,6 @@ std::vector<std::vector<thor::PathInfo>> thor_worker_t::get_path(PathAlgorithm* | |
| } | ||
| } | ||
|
|
||
| return paths; | ||
| } | ||
|
|
||
|
|
@@ -395,38 +427,33 @@ void thor_worker_t::path_arrive_by(Api& api, const std::string& costing) { | |
| GraphId first_edge; | ||
| std::unordered_map<size_t, std::pair<EdgeTrimmingInfo, EdgeTrimmingInfo>> vias; | ||
| std::vector<thor::PathInfo> path; | ||
| std::vector<std::string> algorithms; | ||
| const Options& options = api.options(); | ||
| valhalla::Trip& trip = *api.mutable_trip(); | ||
| trip.mutable_routes()->Reserve(options.alternates() + 1); | ||
|
|
||
| auto route_two_points = [&, this](auto& origin, auto& destination) -> bool { | ||
| // Get the algorithm type for this location pair | ||
| thor::PathAlgorithm* path_algorithm = get_path_algorithm(costing, *origin, *destination, options); | ||
| path_algorithm->Clear(); | ||
| algorithms.push_back(path_algorithm->name()); | ||
| LOG_INFO(std::string("algorithm::") + path_algorithm->name()); | ||
|
|
||
| // TODO: delete this and send all cases to the function above | ||
|
There was a problem hiding this comment. so since we deleted this, then can we delete that function i had lying around above and also delete this comment? |
||
| // If we are continuing through a location we need to make sure we | ||
| // only allow the edge that was used previously (avoid u-turns) | ||
| if (is_through_point(*destination) && first_edge.Is_Valid()) { | ||
| remove_all_edges_except_one(*destination, first_edge); | ||
| } | ||
|
|
||
| // Get best path and keep it | ||
| auto temp_paths = get_path(path_algorithm, *origin, *destination, costing, options); | ||
| if (temp_paths.empty()) | ||
| return false; | ||
kevinkreiser marked this conversation as resolved.
Show resolved
Hide resolved
There was a problem hiding this comment. nit: this line isnt covered because your unit test doesnt use date_time type=2 (arrive_by). you could increase coverage by adding another permutation just in case to make sure that the fix works for arrive_by as well. |
||
|
|
||
| for (auto& temp_path : temp_paths) { | ||
| // back propagate time information | ||
| if (destination->has_date_time() && options.date_time_type() != valhalla::Options::invariant) { | ||
| auto origin_dt = offset_date(*reader, destination->date_time(), temp_path.back().edgeid, | ||
| -temp_path.back().elapsed_cost.secs, temp_path.front().edgeid); | ||
| origin->set_date_time(origin_dt); | ||
|
|
@@ -455,32 +482,29 @@ void thor_worker_t::path_arrive_by(Api& api, const std::string& costing) { | |
|
|
||
| // Build trip path for this leg and add to the result if this | ||
| // location is a BREAK or if this is the last location | ||
| if (is_break_point(*origin)) { | ||
| // Move destination back to the last break and collect the throughs | ||
| std::list<valhalla::Location> throughs; | ||
| while (!is_break_point(*destination)) { | ||
| throughs.push_back(*destination); | ||
| --destination; | ||
| } | ||
|
|
||
| // We have to flip the via indices because we built them in backwards order | ||
| std::remove_reference<decltype(vias)>::type flipped; | ||
kevinkreiser marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| flipped.reserve(vias.size()); | ||
| for (const auto& kv : vias) | ||
| flipped.emplace(path.size() - kv.first, kv.second); | ||
| vias.swap(flipped); | ||
|
|
||
| // Form output information based on path edges | ||
| if (trip.routes_size() == 0 || options.alternates() > 0) { | ||
| route = trip.mutable_routes()->Add(); | ||
| route->mutable_legs()->Reserve(options.locations_size()); | ||
| } | ||
| auto& leg = *route->mutable_legs()->Add(); | ||
| TripLegBuilder::Build(options, controller, *reader, mode_costing, path.begin(), path.end(), | ||
| *origin, *destination, throughs, leg, algorithms, interrupt, &vias); | ||
| path.clear(); | ||
| vias.clear(); | ||
| } | ||
|
|
@@ -489,50 +513,87 @@ void thor_worker_t::path_arrive_by(Api& api, const std::string& costing) { | |
| // if we just made a leg that means we are done recording which algorithms were used | ||
| if (path.empty()) | ||
| algorithms.clear(); | ||
|
|
||
| return true; | ||
| }; | ||
|
|
||
| auto correlated = options.locations(); | ||
| bool allow_retry = true; | ||
|
|
||
| // For each pair of locations | ||
| auto origin = ++correlated.rbegin(); | ||
| while (origin != correlated.rend()) { | ||
| auto destination = std::prev(origin); | ||
| if (!route_two_points(origin, destination)) { | ||
| // if routing failed because an intermediate waypoint was snapped to the low reachability road | ||
| // (such road lies in a small connectivity component that is not connected to other locations) | ||
| // we should leave only high reachability candidates and try to route again | ||
| if (allow_retry && is_through_point(*destination) && | ||
kevinkreiser marked this conversation as resolved.
Show resolved
Hide resolved
There was a problem hiding this comment. cant this happen on other locations types than There was a problem hiding this comment. for There was a problem hiding this comment. haha i made the mistake of leaving this comment on the arrive_by which is harder to think about. let me see if i can mentally find a crack in the forward direction (depart_at). i just need to mull it over a bit to see if it handles everything we're worried about sorry for the confusion, just want to really understand the changes to this critical section ;) |
||
| !is_high_reachable(*destination, destination->path_edges(0))) { | ||
|
There was a problem hiding this comment. the reason why There was a problem hiding this comment. hm, good question. I though that the first (last) point can't be a There was a problem hiding this comment. should we handle it somehow ? |
||
| allow_retry = false; | ||
| // for each intermediate waypoint remove candidates with low reachability | ||
| correlated = options.locations(); | ||
| for (auto loc = std::next(correlated.begin()); loc != std::prev(correlated.end()); ++loc) { | ||
| remove_edges_with_low_reachability(*loc); | ||
| // it doesn't make sense to continue if there are no more path edges | ||
| if (loc->path_edges_size() == 0) | ||
| // no route found | ||
| throw valhalla_exception_t{442}; | ||
| } | ||
| // reset state and try to route again | ||
| route = nullptr; | ||
| first_edge = {}; | ||
| vias.clear(); | ||
| path.clear(); | ||
| algorithms.clear(); | ||
| trip.mutable_routes()->Clear(); | ||
| origin = ++correlated.rbegin(); | ||
| continue; | ||
|
There was a problem hiding this comment. resets the entire state of all the legs of the route and starts completely over from the beginning doing all the legs over. There was a problem hiding this comment. is it proposition for comment ? to make more detailed ? There was a problem hiding this comment. It's just me clarifying my understanding or anyone else's who is following along haha |
||
| } | ||
| // no route found | ||
| throw valhalla_exception_t{442}; | ||
| } | ||
| ++origin; | ||
| } | ||
| // Reverse the legs because protobuf only has adding to the end | ||
| std::reverse(route->mutable_legs()->begin(), route->mutable_legs()->end()); | ||
| // assign changed locations | ||
| *api.mutable_options()->mutable_locations() = std::move(correlated); | ||
| } | ||
|
|
||
| void thor_worker_t::path_depart_at(Api& api, const std::string& costing) { | ||
| // Things we'll need | ||
| TripRoute* route = nullptr; | ||
| GraphId last_edge; | ||
| std::unordered_map<size_t, std::pair<EdgeTrimmingInfo, EdgeTrimmingInfo>> vias; | ||
| std::vector<thor::PathInfo> path; | ||
| std::vector<std::string> algorithms; | ||
| const Options& options = api.options(); | ||
| valhalla::Trip& trip = *api.mutable_trip(); | ||
| trip.mutable_routes()->Reserve(options.alternates() + 1); | ||
|
|
||
| auto route_two_locations = [&, this](auto& origin, auto& destination) -> bool { | ||
| // Get the algorithm type for this location pair | ||
| thor::PathAlgorithm* path_algorithm = get_path_algorithm(costing, *origin, *destination, options); | ||
| path_algorithm->Clear(); | ||
| algorithms.push_back(path_algorithm->name()); | ||
| LOG_INFO(std::string("algorithm::") + path_algorithm->name()); | ||
|
|
||
| // TODO: delete this and send all cases to the function above | ||
| // If we are continuing through a location we need to make sure we | ||
| // only allow the edge that was used previously (avoid u-turns) | ||
| if (is_through_point(*origin) && last_edge.Is_Valid()) { | ||
| remove_all_edges_except_one(*origin, last_edge); | ||
| } | ||
|
|
||
| // Get best path and keep it | ||
| auto temp_paths = get_path(path_algorithm, *origin, *destination, costing, options); | ||
| if (temp_paths.empty()) | ||
| return false; | ||
|
|
||
| for (auto& temp_path : temp_paths) { | ||
| // forward propagate time information | ||
| if (origin->has_date_time() && options.date_time_type() != valhalla::Options::invariant) { | ||
| auto destination_dt = | ||
| offset_date(*reader, origin->date_time(), temp_path.front().edgeid, | ||
| temp_path.back().elapsed_cost.secs, temp_path.back().edgeid); | ||
|
|
@@ -563,23 +624,21 @@ void thor_worker_t::path_depart_at(Api& api, const std::string& costing) { | |
|
|
||
| // Build trip path for this leg and add to the result if this | ||
| // location is a BREAK or if this is the last location | ||
| if (is_break_point(*destination)) { | ||
| // Move origin back to the last break and collect the throughs | ||
| std::list<valhalla::Location> throughs; | ||
| while (!is_break_point(*origin)) { | ||
| throughs.push_front(*origin); | ||
| --origin; | ||
| } | ||
|
|
||
| // Form output information based on path edges. vias are a route discontinuity map | ||
| if (trip.routes_size() == 0 || options.alternates() > 0) { | ||
| route = trip.mutable_routes()->Add(); | ||
| route->mutable_legs()->Reserve(options.locations_size()); | ||
| } | ||
| auto& leg = *route->mutable_legs()->Add(); | ||
| thor::TripLegBuilder::Build(options, controller, *reader, mode_costing, path.begin(), | ||
| path.end(), *origin, *destination, throughs, leg, algorithms, | ||
| interrupt, &vias); | ||
| path.clear(); | ||
|
|
@@ -590,7 +649,50 @@ void thor_worker_t::path_depart_at(Api& api, const std::string& costing) { | |
| // if we just made a leg that means we are done recording which algorithms were used | ||
| if (path.empty()) | ||
| algorithms.clear(); | ||
|
|
||
| return true; | ||
| }; | ||
|
|
||
| auto correlated = options.locations(); | ||
| bool allow_retry = true; | ||
|
|
||
| // For each pair of locations | ||
| auto destination = ++correlated.begin(); | ||
| while (destination != correlated.end()) { | ||
| auto origin = std::prev(destination); | ||
| if (!route_two_locations(origin, destination)) { | ||
| // if routing failed because an intermediate waypoint was snapped to the low reachability road | ||
| // (such road lies in a small connectivity component that is not connected to other locations) | ||
| // we should leave only high reachability candidates and try to route again | ||
| if (allow_retry && is_through_point(*origin) && | ||
| !is_high_reachable(*origin, origin->path_edges(0))) { | ||
|
There was a problem hiding this comment. @kevinkreiser I think such condition if (allow_retry && origin != correlated.begin() && is_through_point(*origin) && origin->path_edges_size() > 0 &&
!is_highly_reachable(*origin, origin->path_edges(0)))should handle all cases |
||
| allow_retry = false; | ||
| // for each intermediate waypoint remove candidates with low reachability | ||
| correlated = options.locations(); | ||
| for (auto loc = std::next(correlated.begin()); loc != std::prev(correlated.end()); ++loc) { | ||
| remove_edges_with_low_reachability(*loc); | ||
| // it doesn't make sense to continue if there are no more path edges | ||
| if (loc->path_edges_size() == 0) | ||
| // no route found | ||
| throw valhalla_exception_t{442}; | ||
| } | ||
| // reset state and try to route again | ||
| route = nullptr; | ||
| last_edge = {}; | ||
| vias.clear(); | ||
| path.clear(); | ||
| algorithms.clear(); | ||
| trip.mutable_routes()->Clear(); | ||
| destination = ++correlated.begin(); | ||
| continue; | ||
| } | ||
| // no route found | ||
| throw valhalla_exception_t{442}; | ||
| } | ||
| ++destination; | ||
| } | ||
| // assign changed locations | ||
| *api.mutable_options()->mutable_locations() = std::move(correlated); | ||
| } | ||
|
|
||
| /** | ||
|
|
||
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
nit: should be
is_highly_reachableorhas_high_reach