add ECBS

Kei18 · Jun 22, 2020 · 64be272 · 64be272
1 parent a73aac1
commit 64be272
Show file tree

Hide file tree

Showing 14 changed files with 1,128 additions and 86 deletions.
diff --git a/instance/map/brc202d.map b/instance/map/brc202d.map
diff --git a/main.cpp b/main.cpp
@@ -8,6 +8,8 @@
 #include <hca.hpp>
 #include <whca.hpp>
 #include <cbs.hpp>
+#include <ecbs.hpp>
+
 
 void printHelp();
 Solver* getSolver(const std::string solver_name,
@@ -94,6 +96,8 @@ Solver* getSolver(const std::string solver_name,
     solver = new WHCA(P);
   } else if (solver_name == "CBS") {
     solver = new CBS(P);
+  } else if (solver_name == "ECBS") {
+    solver = new ECBS(P);
   } else {
     warn("unknown solver name, " + solver_name + ", continue by PIBT");
     solver = new PIBT(P);
@@ -117,4 +121,5 @@ void printHelp() {
   HCA::printHelp();
   WHCA::printHelp();
   CBS::printHelp();
+  ECBS::printHelp();
 }
diff --git a/mapf/CMakeLists.txt b/mapf/CMakeLists.txt
@@ -7,7 +7,8 @@ add_library(lib-mapf STATIC
   ./src/pibt.cpp
   ./src/hca.cpp
   ./src/whca.cpp
-  ./src/cbs.cpp)
+  ./src/cbs.cpp
+  ./src/ecbs.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
@@ -14,6 +14,7 @@ class CBS : public Solver {
 public:
   static const std::string SOLVER_NAME;
 
+protected:
   enum struct OBJECTIVE
     { SOC, MAKESPAN, MAKESPAN_SOC, NUM_ITEMS };
 
@@ -36,13 +37,14 @@ class CBS : public Solver {
   using CompareHighLevelNodes = std::function<bool(HighLevelNode*,
                                                    HighLevelNode*)>;
 
-private:
   OBJECTIVE objective_type;
 
-  HighLevelNode* createInitialHighLevelNode();
-  Constraints getFirstConflict(const HighLevelNode* n);
+  void setInitialHighLevelNode(HighLevelNode* n);
+  Path getInitialPath(int id);
+  Constraints getFirstConflict(const Paths& paths);
   void invoke(HighLevelNode* h_node, int id);
   int countConflict(const Paths& paths);
+  int countConflict(int id, const Path& path, const Paths& _paths);
   Path getConstrainedPath(HighLevelNode* h_node, int id);
   CompareHighLevelNodes returnObjectiveFunc();
 

diff --git a/mapf/include/ecbs.hpp b/mapf/include/ecbs.hpp
@@ -0,0 +1,70 @@
+/*
+ * Implementation of Enhanced Conflict-based Search (ECBS)
+ *
+ * - ref
+ * Barer, M., Sharon, G., Stern, R., & Felner, A. (2014).
+ * Suboptimal Variants of the Conflict-Based Search Algorithm for the Multi-Agent Pathfinding Problem.
+ * In Seventh Annual Symposium on Combinatorial Search.
+ */
+
+#pragma once
+#include "cbs.hpp"
+#include <tuple>
+
+
+class ECBS : public CBS {
+public:
+  static const std::string SOLVER_NAME;
+
+private:
+  struct HighLevelNodeECBS {
+    Paths paths;
+    Constraints constraints;
+    int makespan;
+    int soc;
+    int f;
+    int LB;  // lower bound
+    std::vector<int> f_mins;
+    bool valid;
+  };
+
+  struct FocalNode {
+    Node* v;
+    int g;  // in getTimedPath, g represents t
+    int f1;
+    int f2;
+    FocalNode* p;  // parent
+  };
+  using CompareFocalNode = std::function<bool(FocalNode*, FocalNode*)>;
+  using CheckFocalFin = std::function<bool(FocalNode*)>;
+  using CheckInvalidFocalNode = std::function<bool(FocalNode*)>;
+  using FocalHeuristics = std::function<int(FocalNode*)>;
+
+  float sub_optimality;
+  static const float DEFAULT_SUB_OPTIMALITY;
+
+  void invoke(HighLevelNodeECBS* h_node, int id);
+  void setInitialHighLevelNode(HighLevelNodeECBS* n);
+
+  std::tuple<Path, int> getFocalPath(HighLevelNodeECBS* h_node, int id);
+  std::tuple<Path, int> getTimedPathByFocalSearch
+  (Node* const s, Node* const g, float w,  // sub-optimality
+   FocalHeuristics& f1Value,
+   FocalHeuristics& f2Value,
+   CompareFocalNode& compareOPEN,
+   CompareFocalNode& compareFOCAL,
+   CheckFocalFin& checkFocalFin,
+   CheckInvalidFocalNode& checkInvalidFocalNode);
+
+  Path getPathFromFocalNode(FocalNode* _n);
+
+
+public:
+  ECBS(Problem* _P);
+  ~ECBS() {};
+
+  void solve();
+
+  void setParams(int argc, char *argv[]);
+  static void printHelp();
+};
diff --git a/mapf/include/graph.hpp b/mapf/include/graph.hpp
@@ -20,11 +20,11 @@ struct Pos {
     std::cout << std::endl;
   }
 
-  int manhattanDist(const Pos& pos) {
+  int manhattanDist(const Pos& pos) const {
     return std::abs(x - pos.x) + std::abs(y - pos.y);
   }
 
-  float euclideanDist(const Pos& pos) {
+  float euclideanDist(const Pos& pos) const {
     float dx = x - pos.x;
     float dy = y - pos.y;
     return std::sqrt(dx*dx + dy*dy);
@@ -60,17 +60,17 @@ struct Node {
 
   int getDegree() { return neighbor.size(); }
 
-  float manhattanDist(const Node& node) {
+  float manhattanDist(const Node& node) const {
     return pos.manhattanDist(node.pos);
   }
-  float manhattanDist(const Node* node) {
+  float manhattanDist(Node* const node) const {
     return pos.manhattanDist(node->pos);
   }
 
-  float euclideanDist(const Node& node) {
+  float euclideanDist(const Node& node) const {
     return pos.euclideanDist(node.pos);
   }
-  float euclideanDist(const Node* node) {
+  float euclideanDist(Node* const node) const {
     return pos.euclideanDist(node->pos);
   }
 
@@ -93,8 +93,8 @@ struct Node {
 
   bool operator==(const Node& v) { return v.id  == id; };
   bool operator!=(const Node& v) { return v.id  != id; };
-  bool operator==(const Node* v) { return v->id == id; };
-  bool operator!=(const Node* v) { return v->id != id; };
+  bool operator==(Node* const v) { return v->id == id; };
+  bool operator!=(Node* const v) { return v->id != id; };
 };
 
 using Nodes   = std::vector<Node*>;
@@ -117,7 +117,7 @@ class Graph {
   virtual bool nodeExist(int x, int y) const { return false; };
   virtual Node* getNode(int x, int y) const { return nullptr; };
   virtual Node* getNode(int id) const { return nullptr; };
-  virtual int dist(Node* v, Node* u) { return 0; }
+  virtual int dist(Node* const v, Node* const u) { return 0; }
 
   std::string getMapFileName() { return map_file; };
   int getWidth() { return width; }
@@ -134,5 +134,5 @@ class Grid : public Graph {
   bool nodeExist(int x, int y) const;
   Node* getNode(int x, int y) const;
   Node* getNode(int id) const;
-  int dist(Node* v, Node* u) { return v->manhattanDist(u); }
+  int dist(Node* const v, Node* const u) { return v->manhattanDist(u); }
 };
diff --git a/mapf/include/solver.hpp b/mapf/include/solver.hpp
@@ -56,9 +56,7 @@ struct Paths {
   }
 
   Path get(int i) const {
-    if (!(0 <= i && i < paths.size())) {
-      halt("invalid index.");
-    }
+    if (!(0 <= i && i < paths.size())) halt("invalid index.");
     return paths[i];
   }
 
@@ -111,19 +109,22 @@ struct Paths {
     return makespan;
   }
 
+  int costOfPath(int i) const {
+    if (!(0 <= i && i < paths.size())) halt("invalid index.");
+    int c = paths[i].size();
+    auto itr = paths[i].end() - 1;
+    Node* g = *itr;
+    while (*itr == g) {
+      --c;
+      if (c <= 0) break;
+      --itr;
+    }
+    return c;
+  }
+
   int getSOC() const {
     int soc = 0;
-    for (auto p : paths) {
-      int c = p.size();
-      auto itr = p.end() - 1;
-      Node* g = *itr;
-      while (*itr == g) {
-        --c;
-        if (c <= 0) break;
-        --itr;
-      }
-      soc += c;
-    }
+    for (int i = 0; i < paths.size(); ++i) soc += costOfPath(i);
     return soc;
   }
 
@@ -172,9 +173,9 @@ class Solver {
 private:
   // cache
   std::unordered_map<std::string, Path> PATH_TABLE;
-  static std::string getPathTableKey(Node* s, Node* g);
+  static std::string getPathTableKey(Node* const s, Node* const g);
   void registerPath(const Path& path);
-  Path getPathOnG(Node* s, Node* g);
+  Path getPathOnG(Node* const s, Node* const g);
 
 protected:
   std::string solver_name;
@@ -197,16 +198,18 @@ class Solver {
   using CompareAstarNode = std::function<bool(AstarNode*, AstarNode*)>;
   using CheckAstarFin = std::function<bool(AstarNode*)>;
   using CheckInvalidAstarNode = std::function<bool(AstarNode*)>;
+  using AstarHeuristics = std::function<int(AstarNode*)>;
 
   Plan solution;
   bool solved;
   std::chrono::system_clock::time_point t_start;
   double comp_time;
 
-  Path getPath(Node* s, Node* g);
-  int pathDist(Node* s, Node* g);
-  Path getTimedPath(Node* s,
-                    Node* g,
+  Path getPath(Node* const s, Node* const g);
+  int pathDist(Node* const s, Node* const g);
+  Path getTimedPath(Node* const s,
+                    Node* const g,
+                    AstarHeuristics& fValue,
                     CompareAstarNode& compare,
                     CheckAstarFin& checkAstarFin,
                     CheckInvalidAstarNode& checkInvalidAstarNode);