add CBS

Kei18 · Jun 21, 2020 · a73aac1 · a73aac1
1 parent 4762dc1
commit a73aac1
Show file tree

Hide file tree

Showing 11 changed files with 476 additions and 58 deletions.
diff --git a/instance/makespan-soc-pareto.txt b/instance/makespan-soc-pareto.txt
@@ -0,0 +1,6 @@
+map_file=../instance/map/makespan-soc-pareto.map
+agents=2
+seed=0
+random_problem=0
+3,1,4,1
+1,3,5,1
diff --git a/instance/map/lak105d.map b/instance/map/lak105d.map
@@ -0,0 +1,29 @@
+type octile
+height 25
+width 31
+map
+.....TTTTTTTTTTTTTTTTTTTTTT@@@@
+.....T......T.TTTTTTTTTTTTT@@@@
+.....T....TTT.TTTTTT....TTTT@@@
+.....T.................TTTTTTT@
+.....TTT...............TTTTTTT@
+.....TTT................TTTTTT@
+.....TTT.................T..TT@
+....TTTT............TTT.....TTT
+....TT..............TTT.....TTT
+....TT..............TTT.....TTT
+....TT.......TTTTTTT.......TTT@
+.............TTTTTTT......TTTT@
+.............TTTTTTTTT....TTTT@
+....TT.......TTTTTTT.......TTT@
+....TT.......TTTTTT........TTT@
+....TTTT........TTT.TTT...TTTTT
+.....TTT............TTT...TTTTT
+.....TTT............TTT...TTTTT
+.....TTT.................TTTTT@
+.....TTT.......T.........TTTTT@
+.....TTT......TT........TTTTTT@
+.....T........TT........TTTTT@@
+.....T.........TT.....TTTTT@@@@
+.....TTT..TTTTTTTTTTTTTTTTT@@@@
+@@@@@@@T..TT@@@@@@@@@@@@TT@@@@@
diff --git a/instance/map/makespan-soc-pareto.map b/instance/map/makespan-soc-pareto.map
@@ -0,0 +1,7 @@
+height 4
+width 6
+map
+TTTT.T
+......
+.TT.T.
+..T...
diff --git a/main.cpp b/main.cpp
@@ -7,6 +7,7 @@
 #include <pibt.hpp>
 #include <hca.hpp>
 #include <whca.hpp>
+#include <cbs.hpp>
 
 void printHelp();
 Solver* getSolver(const std::string solver_name,
@@ -91,6 +92,8 @@ Solver* getSolver(const std::string solver_name,
     solver = new HCA(P);
   } else if (solver_name == "WHCA") {
     solver = new WHCA(P);
+  } else if (solver_name == "CBS") {
+    solver = new CBS(P);
   } else {
     warn("unknown solver name, " + solver_name + ", continue by PIBT");
     solver = new PIBT(P);
@@ -113,4 +116,5 @@ void printHelp() {
   PIBT::printHelp();
   HCA::printHelp();
   WHCA::printHelp();
+  CBS::printHelp();
 }
diff --git a/mapf/CMakeLists.txt b/mapf/CMakeLists.txt
@@ -6,7 +6,8 @@ add_library(lib-mapf STATIC
   ./src/solver.cpp
   ./src/pibt.cpp
   ./src/hca.cpp
-  ./src/whca.cpp)
+  ./src/whca.cpp
+  ./src/cbs.cpp)
 target_compile_options(lib-mapf PUBLIC -O3 -Wall -mtune=native -march=native)
 target_compile_features(lib-mapf PUBLIC cxx_std_17)
 target_include_directories(lib-mapf INTERFACE ./include)
diff --git a/mapf/include/cbs.hpp b/mapf/include/cbs.hpp
@@ -0,0 +1,57 @@
+/*
+ * Implementation of Conflict-based Search (CBS)
+ *
+ * - ref
+ * Sharon, G., Stern, R., Felner, A., & Sturtevant, N. R. (2015).
+ * Conflict-based search for optimal multi-agent pathfinding.
+ * Artificial Intelligence, 219, 40–66.
+ */
+
+#pragma once
+#include "solver.hpp"
+
+class CBS : public Solver {
+public:
+  static const std::string SOLVER_NAME;
+
+  enum struct OBJECTIVE
+    { SOC, MAKESPAN, MAKESPAN_SOC, NUM_ITEMS };
+
+  struct Constraint {
+    int id;   // agent id
+    int t;
+    Node* v;  // at t
+    Node* u;  // used only for swap conflict, at t-1
+  };
+  using Constraints = std::vector<Constraint*>;
+
+  struct HighLevelNode {
+    Paths paths;
+    Constraints constraints;
+    int makespan;
+    int soc;
+    int f;   // for tie-break
+    bool valid;
+  };
+  using CompareHighLevelNodes = std::function<bool(HighLevelNode*,
+                                                   HighLevelNode*)>;
+
+private:
+  OBJECTIVE objective_type;
+
+  HighLevelNode* createInitialHighLevelNode();
+  Constraints getFirstConflict(const HighLevelNode* n);
+  void invoke(HighLevelNode* h_node, int id);
+  int countConflict(const Paths& paths);
+  Path getConstrainedPath(HighLevelNode* h_node, int id);
+  CompareHighLevelNodes returnObjectiveFunc();
+
+public:
+  CBS(Problem* _P);
+  ~CBS() {};
+
+  void solve();
+
+  void setParams(int argc, char *argv[]);
+  static void printHelp();
+};
diff --git a/mapf/include/pibt.hpp b/mapf/include/pibt.hpp
@@ -10,33 +10,33 @@
 #pragma once
 #include "solver.hpp"
 
-struct PIBTAgent {
-  int id;
-  Node* v_now;
-  Node* v_next;
-  Node* g;
-  int elapsed;
-  int init_d;
-  float tie_breaker;
-};
-
 class PIBT : public Solver {
 public:
   static const std::string SOLVER_NAME;
 
 private:
+  struct Agent {
+    int id;
+    Node* v_now;
+    Node* v_next;
+    Node* g;
+    int elapsed;
+    int init_d;
+    float tie_breaker;
+  };
+
   // option
   bool disable_dist_init = false;
 
-  bool funcPIBT(PIBTAgent* ai,
-                std::unordered_map<int, PIBTAgent*>& occupied_now,
-                std::unordered_map<int, PIBTAgent*>& occupied_next);
-  Node* planOneStep(PIBTAgent* a,
-                    std::unordered_map<int, PIBTAgent*>& occupied_now,
-                    std::unordered_map<int, PIBTAgent*>& occupied_next);
-  Node* chooseNode(PIBTAgent* a,
-                   std::unordered_map<int, PIBTAgent*>& occupied_now,
-                   std::unordered_map<int, PIBTAgent*>& occupied_next);
+  bool funcPIBT(Agent* ai,
+                std::unordered_map<int, Agent*>& occupied_now,
+                std::unordered_map<int, Agent*>& occupied_next);
+  Node* planOneStep(Agent* a,
+                    std::unordered_map<int, Agent*>& occupied_now,
+                    std::unordered_map<int, Agent*>& occupied_next);
+  Node* chooseNode(Agent* a,
+                   std::unordered_map<int, Agent*>& occupied_now,
+                   std::unordered_map<int, Agent*>& occupied_next);
 
 public:
   PIBT(Problem* _P);

diff --git a/mapf/include/solver.hpp b/mapf/include/solver.hpp
@@ -47,6 +47,7 @@ struct Plan {
 
 struct Paths {
   std::vector<Path> paths;
+  int makespan;
 
   void initialize(int num_agents) {
     paths.clear();
@@ -61,17 +62,26 @@ struct Paths {
     return paths[i];
   }
 
-  void insert(int i, const Path& path) {
-    if (!(0 <= i && i < paths.size())) {
+  Node* get(int i, int t) const {
+    if (!(0 <= i && i < paths.size()) ||
+        !(0 <= t && t <= makespan)) {
       halt("invalid index.");
     }
+    return paths[i][t];
+  }
+
+  void insert(int i, const Path& path) {
+    if (!(0 <= i && i < paths.size())) halt("invalid index.");
+    int old_len = paths[i].size();
     paths[i] = path;
     format();
+    if (paths[i].size() < old_len) shrink();
+    makespan = getMaxLengthPaths();  // update makespan
   }
 
   void operator+=(const Paths& other) {
     if (paths.size() != other.paths.size()) halt("invalid operation.");
-    if (getMakespan() == 0) {  // empty
+    if (makespan == 0) {  // empty
       paths = other.paths;
     } else {
       for (int i = 0; i < paths.size(); ++i) {
@@ -88,7 +98,7 @@ struct Paths {
     }
   }
 
-  int getMakespan() {
+  int getMaxLengthPaths() const {
     int max_val = 0;
     for (auto p : paths) {
       if (p.empty()) continue;
@@ -97,7 +107,11 @@ struct Paths {
     return max_val;
   }
 
-  int getSOC() {
+  int getMakespan() const {
+    return makespan;
+  }
+
+  int getSOC() const {
     int soc = 0;
     for (auto p : paths) {
       int c = p.size();
@@ -114,16 +128,32 @@ struct Paths {
   }
 
   void format() {
-    int makespan = getMakespan();
+    int len = getMaxLengthPaths();
     for (int i = 0; i < paths.size(); ++i) {
       if (paths[i].empty()) continue;
-      while (paths[i].size()-1 != makespan) {
+      while (paths[i].size()-1 != len) {
         paths[i].push_back(*(paths[i].end()-1));
       }
     }
   }
 
-  Plan toPlan() {
+  void shrink() {
+    while (true) {
+      bool shrinkable = true;
+      for (auto p: paths) {
+        if (p.size() <= 1 || *(p.end()-1) != *(p.end()-2)) {
+          shrinkable = false;
+          break;
+        }
+      }
+      if (!shrinkable) break;
+      for (int i = 0; i < paths.size(); ++i) {
+        paths[i].resize(paths[i].size()-1);
+      }
+    }
+  }
+
+  Plan toPlan() const {
     Plan plan;
     int makespan = getMakespan();
     for (int t = 0; t <= makespan; ++t) {
@@ -137,16 +167,6 @@ struct Paths {
   }
 };
 
-struct AstarNode {
-  Node* v;
-  int g;  // in getTimedPath, g represents t
-  int f;
-  AstarNode* p;  // parent
-};
-
-using CompareAstarNode = std::function<bool(AstarNode*, AstarNode*)>;
-using CheckAstarFin = std::function<bool(AstarNode*)>;
-using CheckInvalidAstarNode = std::function<bool(AstarNode*)>;
 
 class Solver {
 private:
@@ -168,6 +188,16 @@ class Solver {
 
   static bool verbose;
 
+  struct AstarNode {
+    Node* v;
+    int g;  // in getTimedPath, g represents t
+    int f;
+    AstarNode* p;  // parent
+  };
+  using CompareAstarNode = std::function<bool(AstarNode*, AstarNode*)>;
+  using CheckAstarFin = std::function<bool(AstarNode*)>;
+  using CheckInvalidAstarNode = std::function<bool(AstarNode*)>;
+
   Plan solution;
   bool solved;
   std::chrono::system_clock::time_point t_start;