modify bugs

Kei18 · Jan 8, 2021 · e8e45d0 · e8e45d0
1 parent 1413066
commit e8e45d0
Show file tree

Hide file tree

Showing 5 changed files with 155 additions and 51 deletions.
diff --git a/mapf/include/ir.hpp b/mapf/include/ir.hpp
@@ -64,6 +64,10 @@ class IR : public Solver
   std::vector<int> getInteractingAgents(const Paths& current_paths,
                                         const int id_largest_gap);
 
+  Path basicSingleAgentPath(const int id, const Plan& plan);
+  Plan refineSinglePath(const Plan& original_plan);
+  Plan refineSinglePath(const int agent, const Plan& original_plan);
+
 public:
   IR(Problem* _P);
   ~IR();

diff --git a/mapf/src/ir.cpp b/mapf/src/ir.cpp
@@ -126,52 +126,54 @@ Plan IR::getInitialPlan()
 Plan IR::refinePlan(const Config& config_s, const Config& config_g,
                     const Plan& current_plan)
 {
-  Paths current_paths = planToPaths(current_plan);
-  auto gap = [&](int i) { return current_paths.costOfPath(i) - pathDist(i); };
-
-  // find agent with largest gap
-  int id_largest_gap = -1;
-  int gap_largest = 0;
-  for (int i = 0; i < P->getNum(); ++i) {
-    if (inArray(i, CLOSE)) continue;
-    int gap_i = gap(i);
-    if (gap_i > gap_largest) {
-      id_largest_gap = i;
-      gap_largest = gap_i;
-    }
-  }
-
-  // failed to find the agent with largest gap
-  if (id_largest_gap == -1) {
-    CLOSE.clear();
-    HIST_GROUP_SIZE.push_back(0);
-    HIST_GAP.push_back(0);
-    return current_plan;
-  }
-
-  // find interacting agents
-  std::vector<int> sample = getInteractingAgents(current_paths, id_largest_gap);
-  HIST_GROUP_SIZE.push_back(sample.size());
-  HIST_GAP.push_back(gap_largest);
-  info("   ", "id=", id_largest_gap, ", gap=", gap_largest,
-       ", interacting size:", sample.size());
-
-  // create problem
-  int comp_time_limit =
-      std::min(max_comp_time - (int)getSolverElapsedTime(), timeout_refinement);
-  if (comp_time_limit <= 0) return current_plan;  // timeout
-  Problem* _P =
-      new Problem(P, config_s, config_g, comp_time_limit, max_timestep);
-
-  // solve
-  auto res = getOptimalPlan(_P, current_plan, sample);
-
-  Plan plan = std::get<1>(res);
-  if (!std::get<0>(res) || plan.getSOC() == current_plan.getSOC()) {
-    CLOSE.push_back(id_largest_gap);
-  }
-  delete _P;
-  return plan;
+  return refineSinglePath(current_plan);
+
+  // Paths current_paths = planToPaths(current_plan);
+  // auto gap = [&](int i) { return current_paths.costOfPath(i) - pathDist(i); };
+
+  // // find agent with largest gap
+  // int id_largest_gap = -1;
+  // int gap_largest = 0;
+  // for (int i = 0; i < P->getNum(); ++i) {
+  //   if (inArray(i, CLOSE)) continue;
+  //   int gap_i = gap(i);
+  //   if (gap_i > gap_largest) {
+  //     id_largest_gap = i;
+  //     gap_largest = gap_i;
+  //   }
+  // }
+
+  // // failed to find the agent with largest gap
+  // if (id_largest_gap == -1) {
+  //   CLOSE.clear();
+  //   HIST_GROUP_SIZE.push_back(0);
+  //   HIST_GAP.push_back(0);
+  //   return current_plan;
+  // }
+
+  // // find interacting agents
+  // std::vector<int> sample = getInteractingAgents(current_paths, id_largest_gap);
+  // HIST_GROUP_SIZE.push_back(sample.size());
+  // HIST_GAP.push_back(gap_largest);
+  // info("   ", "id=", id_largest_gap, ", gap=", gap_largest,
+  //      ", interacting size:", sample.size());
+
+  // // create problem
+  // int comp_time_limit =
+  //     std::min(max_comp_time - (int)getSolverElapsedTime(), timeout_refinement);
+  // if (comp_time_limit <= 0) return current_plan;  // timeout
+  // Problem* _P =
+  //     new Problem(P, config_s, config_g, comp_time_limit, max_timestep);
+
+  // // solve
+  // auto res = getOptimalPlan(_P, current_plan, sample);
+
+  // Plan plan = std::get<1>(res);
+  // if (!std::get<0>(res) || plan.getSOC() == current_plan.getSOC()) {
+  //   CLOSE.push_back(id_largest_gap);
+  // }
+  // delete _P;
+  // return plan;
 }
 
 std::vector<int> IR::getInteractingAgents(const Paths& current_paths,
@@ -191,6 +193,104 @@ std::vector<int> IR::getInteractingAgents(const Paths& current_paths,
   return sample_vec;
 }
 
+Plan IR::refineSinglePath(const Plan& original_plan)
+{
+  auto plan = original_plan;
+  for (int i = 0; i < P->getNum(); ++i) {
+    plan = refineSinglePath(i, plan);
+  }
+  return plan;
+}
+
+Path IR::basicSingleAgentPath(const int id, const Plan& plan)
+{
+  // configuration
+  const auto s = P->getStart(id);
+  const auto g = P->getGoal(id);
+  const auto makespan = plan.getMakespan();
+
+  // calculate single path
+  auto config_s = P->getConfigStart();
+  auto config_g = P->getConfigGoal();
+  int max_constraint_time = 0;  // from when the agent stays its goal
+  for (int i = 0; i < P->getNum(); ++i) {
+    if (i == id) continue;
+    for (int t = 0; t < makespan; ++t) {
+      if (plan.get(t, i) == g) {
+        max_constraint_time = std::max(t, max_constraint_time);
+      }
+    }
+  }
+
+  AstarHeuristics fValue;
+  if (pathDist(id) > max_constraint_time) {
+    fValue = [&](AstarNode* n) { return n->g + pathDist(n->v, g); };
+  } else {
+    // when someone occupies its goal
+    fValue = [&](AstarNode* n) {
+      return std::max(max_constraint_time + 1, n->g + pathDist(n->v, g));
+    };
+  }
+
+  CompareAstarNode compare = [&](AstarNode* a, AstarNode* b) {
+    if (a->f != b->f) return a->f > b->f;
+    if (a->g != b->g) return a->g < b->g;
+    return false;
+  };
+
+  CheckAstarFin checkAstarFin = [&](AstarNode* n) {
+    return n->v == g && n->g > max_constraint_time;
+  };
+
+  CheckInvalidAstarNode checkInvalidAstarNode = [&](AstarNode* m) {
+    for (int i = 0; i < P->getNum(); ++i) {
+      if (i == id) continue;
+      // last node
+      if (m->g > makespan || (m->g == makespan && m->v != g)) return true;
+      // vertex conflict
+      if (plan.get(m->g, i) == m->v) return true;
+      // swap conflict
+      if (plan.get(m->g, i) == m->p->v
+          && plan.get(m->g - 1, i) == m->v) return true;
+    }
+    return false;
+  };
+
+  return getTimedPath(s, g, fValue, compare, checkAstarFin, checkInvalidAstarNode);
+}
+
+Plan IR::refineSinglePath(const int id, const Plan& original_plan)
+{
+  const auto refined_path = basicSingleAgentPath(id, original_plan);
+
+  // compare two paths
+  if (getPathCost(refined_path) < getPathCost(original_plan.getPath(id))) {
+    auto paths = planToPaths(original_plan);
+    paths.insert(id, refined_path);
+    return pathsToPlan(paths);
+  }
+
+  return original_plan;
+}
+
+// Plan IR::refinePathAtGoal(const int id, const Plan& original_plan)
+// {
+//   const Node* g = P->getGoal(id);
+//   const Path original_path = original_plan.getPath(id);
+//   const int original_cost = getPathCost(original_path);
+//   const int real_dist = pathDist(P->getStart(id), g);
+
+//   while (int t = cost - 1; t > real_dist; --t) {
+//     for (int i = 0; i < P->getNum(); ++i) {
+//       if (i == id || original_plan.get(t, i) != g) continue;
+//       if (t - 2 < 0 || original_path[t-2] != g) continue;
+//       // get refined path for i
+//       int length = getPathCost(original_plan.getPath(id));
+//       Path path;
+//     }
+//   }
+// }
+
 std::tuple<bool, Plan> IR::getOptimalPlan(Problem* _P, const Plan& current_plan,
                                           const std::vector<int>& sample)
 {
@@ -360,7 +460,7 @@ void IR::printHelp()
 
       << "  -Y --option-refine-solver [\"OPTION\"]\n"
       << "                                "
-      << "option for refine-solver"
+      << "option for refine-solver\n"
 
       << "  -n --max-iteration [INT]"
       << "      "

diff --git a/mapf/src/paths.cpp b/mapf/src/paths.cpp
@@ -31,7 +31,7 @@ void Paths::insert(int i, const Path& path)
   paths[i] = path;
   if (path.size() - 1 == getMakespan()) return;
   format();                                 // align each path size
-  if (paths[i].size() < old_len) shrink();  // cutoff additional configs
+  if (path.size() < old_len) shrink();      // cutoff additional configs
   makespan = getMaxLengthPaths();           // update makespan
 }
 

diff --git a/readme.md b/readme.md
@@ -1,6 +1,6 @@
 Multi-Agent Path Finding
 ===
-[![Build Status](https://travis-ci.com/Kei18/mapf-IR.svg?token=NJ5EpN7k73FqKbLee887&branch=master)](https://travis-ci.com/Kei18/mapf-IR)
+![test](https://github.com/Kei18/mapf-IR/workflows/test/badge.svg?branch=dev)
 [![MIT License](http://img.shields.io/badge/license-MIT-blue.svg?style=flat)](LICENSE)
 
 A simulator and visualizer of Multi-Agent Path Finding (MAPF), used in a paper "Iterative Refinement for Realtime MAPF".

diff --git a/tests/test_paths.cpp b/tests/test_paths.cpp
@@ -87,12 +87,12 @@ TEST(Paths, conflict)
   paths3.insert(0, { v, u, w });
   paths3.insert(1, { u, v, v });
   paths3.insert(2, { w, w, w });
-  ASSERT_EQ(paths2.countConflict(), 2);
+  ASSERT_EQ(paths3.countConflict(), 2);
 
   // insert agents
   Paths paths4(3);
   paths4.insert(0, { v, u, w });
   paths4.insert(1, { u, v, v });
   paths4.insert(2, { w, w, w });
-  ASSERT_EQ(paths2.countConflict(2, { w, w, u }), 1);
+  ASSERT_EQ(paths4.countConflict(2, { w, w, u }), 1);
 }