move interacting agents

Kei18 · Jan 11, 2021 · ec04160 · ec04160
1 parent 5e80231
commit ec04160
Show file tree

Hide file tree

Showing 4 changed files with 69 additions and 33 deletions.
diff --git a/mapf/include/ir.hpp b/mapf/include/ir.hpp
@@ -62,8 +62,6 @@ class IR : public Solver
   bool stopRefinement();
   Plan refinePlan(const Config& config_s, const Config& config_g,
                   const Plan& current_plan);
-  std::vector<int> getInteractingAgents(const Paths& current_paths,
-                                        const int id_largest_gap);
 
 public:
   IR(Problem* _P);

diff --git a/mapf/include/lib_ir.hpp b/mapf/include/lib_ir.hpp
@@ -1,6 +1,7 @@
 #pragma once
 #include "lib_solver.hpp"
 #include "lib_cbs.hpp"
+#include <set>
 
 
 namespace LibIR
@@ -152,4 +153,30 @@ namespace LibIR
   {
     return identifyInteractingSetByMDD(i, plan, P->getG(), P->getStart(i), P->getGoal(i), MT);
   }
+
+  static std::vector<int> identifyAgentsAtGoal
+  (const int i, const Plan& plan, Graph* G, Node* s, Node* g)
+  {
+    const int cost = plan.getPathCost(i);
+    const int dist = G->pathDist(s, g);
+    const int num  = plan.get(0).size();
+
+    std::set<int> modif_set;
+    for (int t = cost - 1; t >= dist; --t) {
+      for (int j = 0; j < num; ++j) {
+        if (j == i) continue;
+        if (plan.get(t, j) == g) modif_set.insert(j);
+      }
+    }
+    if (!modif_set.empty()) modif_set.insert(i);
+    std::vector<int> moidf_list(modif_set.begin(), modif_set.end());
+    return moidf_list;
+  }
+
+  [[maybe_unused]]
+  static std::vector<int> identifyAgentsAtGoal
+  (const int i, const Plan& plan, Problem* P)
+  {
+    return identifyAgentsAtGoal(i, plan, P->getG(), P->getStart(i), P->getGoal(i));
+  }
 };
diff --git a/mapf/src/ir.cpp b/mapf/src/ir.cpp
@@ -129,20 +129,22 @@ Plan IR::refinePlan(const Config& config_s, const Config& config_g,
   Plan plan = current_plan;
   plan = LibIR::refineSinglePaths(plan, P, getRemainedTime());
   plan = LibIR::refineTwoPathsAtGoal(plan, P, getRemainedTime());
-  for (int i = 0; i < P->getNum(); ++i) {
-    const auto modif_list = LibIR::identifyInteractingSetByMDD(i, plan, P, MT);
-    if (modif_list.empty()) continue;
-
-    info(" ", i, modif_list.size(), plan.getSOC());
-    int comp_time_limit =
-        std::min(max_comp_time - (int)getSolverElapsedTime(), timeout_refinement);
-    if (comp_time_limit <= 0) return plan;  // timeout
-    Problem* _P =
-        new Problem(P, config_s, config_g, comp_time_limit, max_timestep);
-    plan = std::get<1>(getOptimalPlan(_P, plan, modif_list));
-    delete _P;
-  }
   return plan;
+  // for (int i = 0; i < P->getNum(); ++i) {
+  //   const auto modif_list = LibIR::identifyInteractingSetByMDD(i, plan, P, MT);
+  //   const auto modif_list = LibIR::identifyAgentsAtGoal(i, plan, P);
+  //   if (modif_list.empty()) continue;
+
+  //   info(" ", i, modif_list.size(), plan.getSOC());
+  //   int comp_time_limit =
+  //       std::min(max_comp_time - (int)getSolverElapsedTime(), timeout_refinement);
+  //   if (comp_time_limit <= 0) return plan;  // timeout
+  //   Problem* _P =
+  //       new Problem(P, config_s, config_g, comp_time_limit, max_timestep);
+  //   plan = std::get<1>(getOptimalPlan(_P, plan, modif_list));
+  //   delete _P;
+  // }
+  // return plan;
 
   // Paths current_paths = planToPaths(current_plan);
   // auto gap = [&](int i) { return current_paths.costOfPath(i) - pathDist(i); };
@@ -192,24 +194,6 @@ Plan IR::refinePlan(const Config& config_s, const Config& config_g,
   // return plan;
 }
 
-std::vector<int> IR::getInteractingAgents(const Paths& current_paths,
-                                          const int id_largest_gap)
-{
-  int cost_largest_gap = current_paths.costOfPath(id_largest_gap);
-  int dist_largest_gap = pathDist(id_largest_gap);
-  std::set<int> sample = {id_largest_gap};
-  Node* g = P->getGoal(id_largest_gap);
-  for (int t = cost_largest_gap - 1; t >= dist_largest_gap; --t) {
-    for (int i = 0; i < P->getNum(); ++i) {
-      if (i == id_largest_gap) continue;
-      if (current_paths.get(i, t) == g) sample.insert(i);
-    }
-  }
-  std::vector<int> sample_vec(sample.begin(), sample.end());
-  return sample_vec;
-}
-
-
 std::tuple<bool, Plan> IR::getOptimalPlan(Problem* _P, const Plan& current_plan,
                                           const std::vector<int>& sample)
 {

diff --git a/tests/test_lib_ir.cpp b/tests/test_lib_ir.cpp
@@ -116,3 +116,30 @@ TEST(LibIR, identifyInteractingSetByMDD)
   auto modif_list2 = LibIR::identifyInteractingSetByMDD(1, plan, &G, s2, g2);
   ASSERT_EQ(modif_list2.size(), 0);
 }
+
+TEST(libIR, identifyAgentsAtGoal)
+{
+  Grid G("8x8.map");
+
+  Node* a = G.getNode(0);
+  Node* b = G.getNode(1);
+  Node* c = G.getNode(2);
+  Node* d = G.getNode(3);
+  Node* e = G.getNode(4);
+  Node* x = G.getNode(3, 1);
+
+  Config starts = { c, a };
+  Config goals  = { d, e };
+  Plan plan;
+  plan.add({ c, a });
+  plan.add({ d, b });
+  plan.add({ d, c });
+  plan.add({ x, d });
+  plan.add({ d, e });
+  ASSERT_TRUE(plan.validate(starts, goals));
+
+  auto modif_list1 = LibIR::identifyAgentsAtGoal(0, plan, &G, c, d);
+  ASSERT_EQ(modif_list1.size(), 2);
+  auto modif_list2 = LibIR::identifyAgentsAtGoal(1, plan, &G, a, e);
+  ASSERT_EQ(modif_list2.size(), 0);
+}