Debug/task planner candidates

rmf_task/src/rmf_task/agv/TaskPlanner.cpp

@@ -193,7 +193,7 @@ class Candidates
   // Map finish time to Entry
   using Map = std::multimap<rmf_traffic::Time, Entry>;
 
-  static Candidates make(
+  static std::shared_ptr<Candidates> make(
     const std::vector<State>& initial_states,
     const std::vector<StateConfig>& state_configs,
     const rmf_task::Request& request,
@@ -283,7 +283,7 @@ class Candidates
   }
 };
 
-Candidates Candidates::make(
+std::shared_ptr<Candidates> Candidates::make(
   const std::vector<State>& initial_states,
   const std::vector<StateConfig>& state_configs,
   const rmf_task::Request& request,
@@ -317,51 +317,82 @@ Candidates Candidates::make(
       {
         auto new_finish = request.estimate_finish(
           battery_estimate.value().finish_state(), state_config, estimate_cache);
-        assert(new_finish.has_value());
-        initial_map.insert(
-          {new_finish.value().finish_state().finish_time(),
-          Entry{
-            i,
-            new_finish.value().finish_state(),
-            new_finish.value().wait_until(),
-            state,
-            true}});
-      }
-      else
-      {
-        std::cerr << "Unable to create entry for candidate [" << i 
-                  << "] and request [" << request.id() << " ]" << std::endl;
-        assert(false);
+        if (new_finish.has_value())
+        {
+          initial_map.insert(
+            {new_finish.value().finish_state().finish_time(),
+            Entry{
+              i,
+              new_finish.value().finish_state(),
+              new_finish.value().wait_until(),
+              state,
+              true}});
+        }
       }
     }
 
   }
 
-  return Candidates(std::move(initial_map));
+  if (initial_map.empty())
+  {
+    std::cout << "Unable to create candidates for request [" << request.id()
+              << "]" << std::endl;
+    return nullptr;
+  }
+
+  std::shared_ptr<Candidates> candidates(
+    new Candidates(std::move(initial_map)));
+  return candidates;
 }
 
 // ============================================================================
-struct PendingTask
+class PendingTask
 {
-  PendingTask(
+public:
+
+  static std::shared_ptr<PendingTask> make(
       std::vector<rmf_task::agv::State>& initial_states,
       std::vector<rmf_task::agv::StateConfig>& state_configs,
       rmf_task::ConstRequestPtr request_,
       rmf_task::ConstRequestPtr charge_battery_request,
-      std::shared_ptr<EstimateCache> estimate_cache)
+      std::shared_ptr<EstimateCache> estimate_cache);
+
+  rmf_task::ConstRequestPtr request;
+  Candidates candidates;
+  rmf_traffic::Time earliest_start_time;
+
+private:
+  PendingTask(
+      rmf_task::ConstRequestPtr request_,
+      Candidates candidates_)
     : request(std::move(request_)),
-      candidates(Candidates::make(initial_states, state_configs,
-        *request, *charge_battery_request, estimate_cache)),
+      candidates(candidates_),
       earliest_start_time(request->earliest_start_time())
   {
     // Do nothing
   }
-
-  rmf_task::ConstRequestPtr request;
-  Candidates candidates;
-  rmf_traffic::Time earliest_start_time;
 };
 
+std::shared_ptr<PendingTask> PendingTask::make(
+    std::vector<rmf_task::agv::State>& initial_states,
+    std::vector<rmf_task::agv::StateConfig>& state_configs,
+    rmf_task::ConstRequestPtr request_,
+    rmf_task::ConstRequestPtr charge_battery_request,
+    std::shared_ptr<EstimateCache> estimate_cache)
+{
+  const auto candidates = Candidates::make(initial_states, state_configs,
+        *request_, *charge_battery_request, estimate_cache);
+
+  if (!candidates)
+    return nullptr;
+
+  std::shared_ptr<PendingTask> pending_task(
+    new PendingTask(request_, *candidates));
+  return pending_task;
+}
+
+
+
 // ============================================================================
 struct Node
 {
@@ -726,6 +757,8 @@ class TaskPlanner::Implementation
     assert(initial_states.size() == state_configs.size());
 
     auto node = make_initial_node(initial_states, state_configs, requests, time_now);
+    if (!node)
+      return {};
 
     TaskPlanner::Assignments complete_assignments;
     complete_assignments.resize(node->assigned_tasks.size());
@@ -777,6 +810,8 @@ class TaskPlanner::Implementation
       }
 
       node = make_initial_node(estimates, state_configs, new_tasks, time_now);
+      if (!node)
+        return {};
       initial_states = estimates;
     }
 
@@ -871,10 +906,20 @@ class TaskPlanner::Implementation
       // Generate a unique internal id for the request. Currently, multiple
       // requests with the same string id will be assigned different internal ids
       std::size_t internal_id = initial_node->get_available_internal_id();
+      const auto pending_task= PendingTask::make(
+          initial_states,
+          state_configs,
+          request,
+          charge_battery,
+          estimate_cache);
+
+      if (!pending_task)
+        return nullptr;
+
       initial_node->unassigned_tasks.insert(
         {
           internal_id,
-          PendingTask(initial_states, state_configs, request, charge_battery, estimate_cache)
+          *pending_task
         });
     }
 

rmf_task/test/unit/agv/test_TaskPlanner.cpp

@@ -21,6 +21,7 @@
 #include <rmf_task/agv/StateConfig.hpp>
 #include <rmf_task/requests/Delivery.hpp>
 #include <rmf_task/requests/ChargeBattery.hpp>
+#include <rmf_task/requests/Loop.hpp>
 
 #include <rmf_traffic/agv/Graph.hpp>
 #include <rmf_traffic/Trajectory.hpp>
@@ -766,4 +767,68 @@ SCENARIO("Grid World")
     REQUIRE(optimal_cost <= greedy_cost);
   }
 
+  WHEN("A loop request is impossible to fulfil")
+  {
+    const auto now = std::chrono::steady_clock::now();
+    const double default_orientation = 0.0;
+
+    rmf_traffic::agv::Plan::Start first_location{now, 13, default_orientation};
+
+    std::vector<rmf_task::agv::State> initial_states =
+    {
+      rmf_task::agv::State{first_location, 9, 1.0},
+    };
+
+    std::vector<rmf_task::agv::StateConfig> state_configs =
+    {
+      rmf_task::agv::StateConfig{0.2},
+    };
+
+    std::vector<rmf_task::ConstRequestPtr> requests =
+    {
+      rmf_task::requests::Loop::make(
+        "Loop1",
+        0,
+        15,
+        1000,
+        motion_sink,
+        device_sink,
+        planner,
+        now,
+        true)
+    };
+
+    std::shared_ptr<rmf_task::agv::TaskPlanner::Configuration>  task_config =
+      std::make_shared<rmf_task::agv::TaskPlanner::Configuration>(
+        battery_system,
+        motion_sink,
+        device_sink,
+        planner);
+    rmf_task::agv::TaskPlanner task_planner(task_config);
+
+
+    auto start_time = std::chrono::steady_clock::now();
+    const auto greedy_assignments = task_planner.greedy_plan(
+      now, initial_states, state_configs, requests);
+    const double greedy_cost = task_planner.compute_cost(greedy_assignments);
+    auto finish_time = std::chrono::steady_clock::now();
+    std::cout << "Greedy solution found in: "
+              << (finish_time - start_time).count() / 1e9 << std::endl;
+    display_solution("Greedy", greedy_assignments, greedy_cost);
+
+    task_planner = rmf_task::agv::TaskPlanner(task_config);
+    start_time = std::chrono::steady_clock::now();
+    const auto optimal_assignments = task_planner.optimal_plan(
+      now, initial_states, state_configs, requests, nullptr);
+    const double optimal_cost = task_planner.compute_cost(optimal_assignments);
+    finish_time = std::chrono::steady_clock::now();
+    std::cout << "Optimal solution found in: "
+              << (finish_time - start_time).count() / 1e9 << std::endl;
+    display_solution("Optimal", optimal_assignments, optimal_cost);
+
+  REQUIRE(optimal_assignments.empty());
+  REQUIRE(greedy_assignments.empty());
+  REQUIRE(optimal_cost <= greedy_cost);
+  }
+
 }