Fix quickest path function to account for closed lanes (#85)

Signed-off-by: Luca Della Vedova <[email protected]> Signed-off-by: Michael X. Grey <[email protected]> Co-authored-by: Michael X. Grey <[email protected]>
open-rmf · Aug 30, 2022 · dc17527 · dc17527
1 parent 295f405
commit dc17527
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 12 deletions.
diff --git a/rmf_traffic/src/rmf_traffic/agv/planning/ShortestPathHeuristic.cpp b/rmf_traffic/src/rmf_traffic/agv/planning/ShortestPathHeuristic.cpp
@@ -33,13 +33,17 @@ void expand_lane(
   FrontierTemplate<NodePtrT, C>& frontier,
   std::unordered_map<WaypointId, NodePtrT>& visited,
   const Graph::Implementation& g,
+  const LaneClosure& closures,
   const double agent_max_speed,
   const std::vector<std::size_t>& lanes)
 {
   const auto& wp_0 = g.waypoints[top->waypoint];
   const auto& p_0 = wp_0.get_location();
   for (const auto l : lanes)
   {
+    if (closures.is_closed(l))
+      continue;
+
     const auto& next_lane = g.lanes[l];
     const auto next_waypoint = GetNextWaypoint()(next_lane);
     if (visited.count(next_waypoint) != 0)
@@ -109,8 +113,9 @@ ShortestPath::ForwardNodePtr ShortestPath::ForwardExpander::expand(
   }
 
   const auto& g = _graph->original();
+  const auto& closures = _graph->closures();
   expand_lane<ForwardGetNextWaypoint>(
-    top, frontier, visited, g, _max_speed,
+    top, frontier, visited, g, closures, _max_speed,
     g.lanes_from[top->waypoint]);
 
   return top;
@@ -178,8 +183,9 @@ ShortestPath::ReverseNodePtr ShortestPath::ReverseExpander::expand(
   }
 
   const auto& g = _graph->original();
+  const auto& closures = _graph->closures();
   expand_lane<ReverseGetNextWaypoint>(
-    top, frontier, visited, g, _max_speed,
+    top, frontier, visited, g, closures, _max_speed,
     g.lanes_into[top->waypoint]);
 
   return top;

diff --git a/rmf_traffic/test/unit/agv/test_Planner.cpp b/rmf_traffic/test/unit/agv/test_Planner.cpp
@@ -746,6 +746,7 @@ SCENARIO("Quickest Path")
   std::size_t goal_wp = 0;
   std::vector<std::size_t> expected_path;
   std::optional<double> expected_cost;
+  auto planner_config = rmf_traffic::agv::Planner::Configuration(graph, traits);
   const rmf_traffic::Time time = std::chrono::steady_clock::now();
 
   GIVEN("Quickest Path Request from 3 -> 7")
@@ -762,17 +763,17 @@ SCENARIO("Quickest Path")
     WHEN("Modest speed limit along 8 -> 10")
     {
       const auto speed = 2.0/3.0;
-      graph.get_lane(18).properties().speed_limit(speed);
-      graph.get_lane(20).properties().speed_limit(speed);
+      planner_config.graph().get_lane(18).properties().speed_limit(speed);
+      planner_config.graph().get_lane(20).properties().speed_limit(speed);
       expected_path = {3, 8, 9, 10, 7};
       expected_cost = 2.0 / speed + 2.0 * diagonal_time;
     }
 
     WHEN("Significant speed limit along 8 -> 10")
     {
       const auto speed = 0.5;
-      graph.get_lane(18).properties().speed_limit(speed);
-      graph.get_lane(20).properties().speed_limit(speed);
+      planner_config.graph().get_lane(18).properties().speed_limit(speed);
+      planner_config.graph().get_lane(20).properties().speed_limit(speed);
 
       // The route will change to following the bottom instead
       expected_path = {3, 4, 0, 1, 2, 6, 7};
@@ -796,14 +797,14 @@ SCENARIO("Quickest Path")
 
     WHEN("Significant speed limit from 6 -> 7")
     {
-      graph.get_lane(12).properties().speed_limit(0.1);
+      planner_config.graph().get_lane(12).properties().speed_limit(0.1);
       expected_path = {5, 4, 3, 8, 9};
       expected_cost = 3.5 + diagonal_time;
     }
 
     WHEN("Significant speed limit on offset lane")
     {
-      graph.get_lane(6).properties().speed_limit(0.1);
+      planner_config.graph().get_lane(6).properties().speed_limit(0.1);
       expected_path = {5, 4, 3, 8, 9};
       expected_cost = 3.5 + diagonal_time;
     }
@@ -838,11 +839,21 @@ SCENARIO("Quickest Path")
     expected_cost = std::nullopt;
   }
 
+  GIVEN("3 -> 7 that is impossible because of blocked lanes")
+  {
+    auto lane_closures = rmf_traffic::agv::LaneClosure();
+    start_set.push_back({time, 3, 0.0});
+    goal_wp = 7;
+    // All the graph is closed
+    for (std::size_t i = 0; i < planner_config.graph().num_lanes(); ++i)
+      lane_closures.close(i);
+    planner_config.lane_closures(lane_closures);
+    // There should be no solution
+    expected_cost = std::nullopt;
+  }
+
   auto options = rmf_traffic::agv::Planner::Options{nullptr};
-  rmf_traffic::agv::Planner planner{
-    rmf_traffic::agv::Planner::Configuration{graph, traits},
-    options
-  };
+  rmf_traffic::agv::Planner planner{planner_config, options};
 
   auto quickest = planner.quickest_path(start_set, goal_wp);
   if (expected_cost.has_value())