Merge pull request #49 from whoenig/mapf/c

Mapf/c

.github/workflows/build.yml

@@ -22,7 +22,8 @@ jobs:
 
     - name: install dependencies
       run: |
-        sudo apt-get install -y g++ cmake libboost-program-options-dev libyaml-cpp-dev clang-tidy clang-format python3-matplotlib graphviz doxygen
+        sudo apt-get install -y g++ cmake libboost-program-options-dev libyaml-cpp-dev clang-tidy clang-format graphviz doxygen
+        pip3 install cvxpy matplotlib numpy
 
     - name: Configure CMake
       # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make.

README.md

@@ -56,3 +56,14 @@ python3 ../test/test_next_best_assignment.py TestNextBestAssignment.test_1by2
 ./ecbs -i ../benchmark/32x32_obst204/map_32by32_obst204_agents10_ex1.yaml -o output.yaml -w 1.3
 python3 ../example/visualize.py ../benchmark/32x32_obst204/map_32by32_obst204_agents10_ex1.yaml output.yaml
 ````
+
+### Generalized Roadmaps
+
+CBS works on generalized graphs, with a particular focus on optional wait actions (e.g., this can be used with motion primitives as well).
+However, the roadmap annotation and visualization step currently assume a 2D Euclidean embedding and straight-line edges.
+
+```
+python3 ../tools/annotate_roadmap.py ../test/mapf_simple1_roadmap_to_annotate.yaml mapf_simple1_roadmap_annotated.yaml
+./cbs_roadmap -i mapf_simple1_roadmap_annotated.yaml -o output.yaml
+python3 ../example/visualize_roadmap.py mapf_simple1_roadmap_annotated.yaml output.yaml
+```

example/cbs_roadmap.cpp

@@ -220,6 +220,12 @@ class Environment {
         m_lowLevelExpanded(0),
         m_disappearAtGoal(disappearAtGoal)
   {
+    auto V = boost::num_vertices(m_roadmap);
+    // Upper bound on the makespan, see
+    // Jingjin Yu, Daniela Rus:
+    // Pebble Motion on Graphs with Rotations: Efficient Feasibility Tests and Planning Algorithms. WAFR 2014: 729-746
+    // NOTE: that the constant factor is not known
+    m_timeLimit = pow(V, 3);
   }
 
   Environment(const Environment&) = delete;
@@ -255,6 +261,11 @@ class Environment {
     //   std::endl;
     // }
     neighbors.clear();
+
+    if (s.time > m_timeLimit) {
+      return;
+    }
+
     auto es = boost::out_edges(s.vertex, m_roadmap);
     for (auto eit = es.first; eit != es.second; ++eit) {
       vertex_t v = boost::target(*eit, m_roadmap);
@@ -265,14 +276,14 @@ class Environment {
       }
     }
 
-    // Wait action
-    {
-      State n(s.time + 1, s.vertex);
-      if (stateValid(n)) {
-        neighbors.emplace_back(
-            Neighbor<State, Action, int>(n, edge_t(), 1));
-      }
-    }
+    // // Wait action
+    // {
+    //   State n(s.time + 1, s.vertex);
+    //   if (stateValid(n)) {
+    //     neighbors.emplace_back(
+    //         Neighbor<State, Action, int>(n, edge_t(), 1));
+    //   }
+    // }
   }
 
   bool getFirstConflict(
@@ -299,7 +310,7 @@ class Environment {
           }
         }
       }
-      // edge/edge (swap)
+      // edge/edge
       for (size_t i = 0; i < solution.size(); ++i) {
         for (size_t j = i + 1; j < solution.size(); ++j) {
           if (t < solution[i].actions.size() &&
@@ -395,6 +406,7 @@ class Environment {
   int m_highLevelExpanded;
   int m_lowLevelExpanded;
   bool m_disappearAtGoal;
+  int m_timeLimit;
 };
 
 int main(int argc, char* argv[]) {
@@ -428,10 +440,24 @@ int main(int argc, char* argv[]) {
 
   YAML::Node config = YAML::LoadFile(inputFile);
 
+  // some sanity checks
+  if (config["roadmap"]["conflicts"]) {
+    if (config["roadmap"]["undirected"].as<bool>()) {
+      throw std::runtime_error("If conflicts are specified, the roadmap cannot be undirected!");
+    }
+    if (config["roadmap"]["allow_wait_actions"].as<bool>()) {
+      throw std::runtime_error("If conflicts are specified, the wait actions must be already encoded in the edge set!");
+    }
+    if (config["roadmap"]["conflicts"].size() != config["roadmap"]["edges"].size()) {
+      throw std::runtime_error("If conflicts are specified, the cardinality of conflicts and edges must match!");
+    }
+  }
+
   // read roadmap
   roadmap_t roadmap;
   std::unordered_map<std::string, vertex_t> vertexMap;
 
+  std::vector<edge_t> edgeVec;
   for (const auto& edge : config["roadmap"]["edges"]) {
     // find or insert vertex1
     auto v1str = edge[0].as<std::string>();
@@ -456,13 +482,33 @@ int main(int argc, char* argv[]) {
       roadmap[v2].name = v2str;
     }
     auto e1 = boost::add_edge(v1, v2, roadmap);
-    if (config["roadmap"]["undirected"]) {
+    edgeVec.push_back(e1.first);
+    if (config["roadmap"]["undirected"].as<bool>()) {
       auto e2 = boost::add_edge(v2, v1, roadmap);
+      edgeVec.push_back(e2.first);
       roadmap[e1.first].conflictingEdges.insert(e2.first);
       roadmap[e2.first].conflictingEdges.insert(e1.first);
     }
   }
 
+  if (config["roadmap"]["allow_wait_actions"].as<bool>()) {
+    for (const auto& v : vertexMap) {
+      auto e = boost::add_edge(v.second, v.second, roadmap);
+      edgeVec.push_back(e.first);
+    }
+  }
+
+  if (config["roadmap"]["conflicts"]) {
+    size_t i = 0;
+    for (const auto& conflicts : config["roadmap"]["conflicts"]) {
+      for (const auto& conflict : conflicts) {
+        size_t j = conflict.as<size_t>();
+        roadmap[edgeVec[i]].conflictingEdges.insert(edgeVec[j]);
+      }
+      ++i;
+    }
+  }
+
   // read agents
   std::vector<vertex_t> goalVertices;
   std::vector<State> startStates;
@@ -493,6 +539,7 @@ int main(int argc, char* argv[]) {
 
     std::ofstream out(outputFile);
     out << "statistics:" << std::endl;
+    out << "  success: " << true << std::endl;
     out << "  cost: " << cost << std::endl;
     out << "  makespan: " << makespan << std::endl;
     out << "  runtime: " << timer.elapsedSeconds() << std::endl;
@@ -517,6 +564,9 @@ int main(int argc, char* argv[]) {
     }
   } else {
     std::cout << "Planning NOT successful!" << std::endl;
+    std::ofstream out(outputFile);
+    out << "statistics:" << std::endl;
+    out << "  success: " << false << std::endl;
   }
 
   return 0;

example/visualize_roadmap.py

@@ -0,0 +1,172 @@
+#!/usr/bin/env python3
+import yaml
+import matplotlib
+# matplotlib.use("Agg")
+from matplotlib.patches import Circle, FancyArrow, Rectangle, Arrow
+from matplotlib.collections import PatchCollection
+import matplotlib.pyplot as plt
+import numpy as np
+from matplotlib import animation
+import matplotlib.animation as manimation
+import argparse
+import math
+
+Colors = ['orange', 'blue', 'green']
+
+
+class Animation:
+  def __init__(self, map, schedule, radius):
+    self.map = map
+    self.schedule = schedule
+    self.radius = radius
+
+    # find boundary
+    all_pos = np.array(list(map["roadmap"]["vertices"].values()))
+    print(all_pos)
+    xmin = np.min(all_pos[:, 0])
+    ymin = np.min(all_pos[:, 1])
+    xmax = np.max(all_pos[:, 0])
+    ymax = np.max(all_pos[:, 1])
+
+    aspect = (xmax - xmin) / (ymax - ymin)
+
+    self.fig = plt.figure(frameon=False, figsize=(4 * aspect, 4))
+    self.ax = self.fig.add_subplot(111, aspect='equal')
+    self.fig.subplots_adjust(left=0,right=1,bottom=0,top=1, wspace=None, hspace=None)
+
+    self.patches = []
+    self.artists = []
+    self.agents = dict()
+    self.agent_names = dict()
+
+    plt.xlim(xmin - radius, xmax + radius)
+    plt.ylim(ymin - radius, ymax + radius)
+
+    v_dict = map["roadmap"]["vertices"]
+    for edge in map["roadmap"]["edges"]:
+      start = v_dict[edge[0]]
+      goal = v_dict[edge[1]]
+      self.patches.append(FancyArrow(start[0], start[1], goal[0] - start[0], goal[1] - start[1], width=0.05, length_includes_head=True))#, head_width=0))
+
+    # create agents:
+    self.T = 0
+    # draw goals first
+    for d, i in zip(map["agents"], range(0, len(map["agents"]))):
+      if "goal" in d:
+        goals = [d["goal"]]
+      if "potentialGoals" in d:
+        goals = [goal for goal in d["potentialGoals"]]
+      for goal in goals:
+        v = v_dict[goal]
+        self.patches.append(Rectangle((v[0] - radius, v[1] - radius), 2*radius, 2*radius, facecolor=Colors[i%len(Colors)], edgecolor='black', alpha=0.5))
+
+    for d, i in zip(map["agents"], range(0, len(map["agents"]))):
+      name = d["name"]
+      v = v_dict[d["start"]]
+      self.agents[name] = Circle((v[0], v[1]), radius, facecolor=Colors[i%len(Colors)], edgecolor='black')
+      self.agents[name].original_face_color = Colors[i%len(Colors)]
+      self.patches.append(self.agents[name])
+      self.T = max(self.T, schedule["schedule"][name][-1]["t"])
+      self.agent_names[name] = self.ax.text(v[0], v[1], name.replace('agent', ''))
+      self.agent_names[name].set_horizontalalignment('center')
+      self.agent_names[name].set_verticalalignment('center')
+      self.artists.append(self.agent_names[name])
+
+    self.anim = animation.FuncAnimation(self.fig, self.animate_func,
+                               init_func=self.init_func,
+                               frames=int(self.T+1) * 10,
+                               interval=100,
+                               blit=True)
+
+  def save(self, file_name, speed):
+    self.anim.save(
+      file_name,
+      "ffmpeg",
+      fps=10 * speed,
+      dpi=200),
+      # savefig_kwargs={"pad_inches": 0, "bbox_inches": "tight"})
+
+  def show(self):
+    plt.show()
+
+  def init_func(self):
+    for p in self.patches:
+      self.ax.add_patch(p)
+    for a in self.artists:
+      self.ax.add_artist(a)
+    return self.patches + self.artists
+
+  def animate_func(self, i):
+    for agent_name in self.schedule["schedule"]:
+      agent = self.schedule["schedule"][agent_name]
+      pos = self.getState(i / 10, agent)
+      p = (pos[0], pos[1])
+      self.agents[agent_name].center = p
+      self.agent_names[agent_name].set_position(p)
+
+    # reset all colors
+    for _,agent in self.agents.items():
+      agent.set_facecolor(agent.original_face_color)
+
+    # check drive-drive collisions
+    agents_array = [agent for _,agent in self.agents.items()]
+    for i in range(0, len(agents_array)):
+      for j in range(i+1, len(agents_array)):
+        d1 = agents_array[i]
+        d2 = agents_array[j]
+        pos1 = np.array(d1.center)
+        pos2 = np.array(d2.center)
+        if np.linalg.norm(pos1 - pos2) < 2 * self.radius:
+          d1.set_facecolor('red')
+          d2.set_facecolor('red')
+          print("COLLISION! (agent-agent) ({}, {})".format(i, j))
+
+    return self.patches + self.artists
+
+
+  def getState(self, t, d):
+    v_dict = self.map["roadmap"]["vertices"]
+    idx = 0
+    while idx < len(d) and d[idx]["t"] < t:
+      idx += 1
+    if idx == 0:
+      return np.array(v_dict[d[0]["v"]])
+    elif idx < len(d):
+      posLast = np.array(v_dict[d[idx-1]["v"]])
+      posNext = np.array(v_dict[d[idx]["v"]])
+    else:
+      return np.array(v_dict[d[-1]["v"]])
+    dt = d[idx]["t"] - d[idx-1]["t"]
+    t = (t - d[idx-1]["t"]) / dt
+    pos = (posNext - posLast) * t + posLast
+    return pos
+
+def main():
+  parser = argparse.ArgumentParser()
+  parser.add_argument("map", help="input file containing map")
+  parser.add_argument("schedule", help="schedule for agents")
+  parser.add_argument('--video', dest='video', default=None, help="output video file (or leave empty to show on screen)")
+  parser.add_argument("--speed", type=int, default=1, help="speedup-factor")
+  parser.add_argument("--radius", type=float, default=0.3, help="radius of robot")
+  args = parser.parse_args()
+
+  with open(args.map) as map_file:
+    map = yaml.safe_load(map_file)
+
+  if "roadmap" not in map:
+    print("Not a roadmap file!")
+    exit()
+
+  with open(args.schedule) as states_file:
+    schedule = yaml.safe_load(states_file)
+
+  animation = Animation(map, schedule, args.radius)
+
+  if args.video:
+    animation.save(args.video, args.speed)
+  else:
+    animation.show()
+
+if __name__ == "__main__":
+  main()
+

test/issue33.yaml

@@ -0,0 +1,4 @@
+{agents: [{goal: '4', name: agent0, start: '0'}], roadmap: {allow_wait_actions: true,
+    edges: [['0', '1'], ['0', '3'], ['0', '2'], ['1', '2'], ['2', '3']], undirected: true,
+    vertices: {'0': [0.0, 0.0], '1': [1.0, 0.0], '2': [1.0, 1.0], '3': [0.0, 1.0],
+      '4': [2.0, 2.0]}}}

test/mapf_atGoal_roadmap.yaml

@@ -1,5 +1,6 @@
 roadmap:
   undirected: True
+  allow_wait_actions: True
   edges:
     - [A, B]
     - [B, C]

test/mapf_circle_roadmap.yaml

@@ -1,5 +1,6 @@
 roadmap:
   undirected: True
+  allow_wait_actions: True
   edges:
     - [A, B]
     - [B, C]

test/mapf_crossover_roadmap.yaml

@@ -0,0 +1,14 @@
+roadmap:
+  undirected: True
+  allow_wait_actions: True
+  edges:
+    - [A, B]
+    - [B, C]
+    - [C, D]
+agents:
+  - name: agent0
+    start: A
+    goal: C
+  - name: agent1
+    start: D
+    goal: B

test/mapf_simple1_roadmap.yaml

@@ -1,5 +1,6 @@
 roadmap:
   undirected: True
+  allow_wait_actions: True
   edges:
     - [A, B]
     - [B, C]