Fix infinite loop in CBS.

centralized/cbs/a_star.py

@@ -6,12 +6,16 @@
 
 """
 
-class AStar():
-    def __init__(self, env):
-        self.agent_dict = env.agent_dict
-        self.admissible_heuristic = env.admissible_heuristic
-        self.is_at_goal = env.is_at_goal
-        self.get_neighbors = env.get_neighbors
+from typing import TYPE_CHECKING
+
+from cbs.constraints import Constraints, VertexConstraint
+
+if TYPE_CHECKING:
+    from cbs.cbs import Environment
+
+class AStar:
+    def __init__(self, env: 'Environment'):
+        self.env = env
 
     def reconstruct_path(self, came_from, current):
         total_path = [current]
@@ -24,7 +28,7 @@ def search(self, agent_name):
         """
         low level search 
         """
-        initial_state = self.agent_dict[agent_name]["start"]
+        initial_state = self.env.agent_dict[agent_name]["start"]
         step_cost = 1
 
         closed_set = set()
@@ -37,19 +41,24 @@ def search(self, agent_name):
 
         f_score = {} 
 
-        f_score[initial_state] = self.admissible_heuristic(initial_state, agent_name)
+        f_score[initial_state] = self.env.admissible_heuristic(initial_state, agent_name)
 
         while open_set:
             temp_dict = {open_item:f_score.setdefault(open_item, float("inf")) for open_item in open_set}
             current = min(temp_dict, key=temp_dict.get)
 
-            if self.is_at_goal(current, agent_name):
-                return self.reconstruct_path(came_from, current)
+            if self.env.is_at_goal(current, agent_name):
+                conflict = any(
+                    vc.time >= current.time and vc.location == current.location
+                    for vc in self.env.constraints.vertex_constraints
+                )
+                if not conflict:
+                    return self.reconstruct_path(came_from, current)
 
             open_set -= {current}
             closed_set |= {current}
 
-            neighbor_list = self.get_neighbors(current)
+            neighbor_list = self.env.get_neighbors(current)
 
             for neighbor in neighbor_list:
                 if neighbor in closed_set:
@@ -65,6 +74,6 @@ def search(self, agent_name):
                 came_from[neighbor] = current
 
                 g_score[neighbor] = tentative_g_score
-                f_score[neighbor] = g_score[neighbor] + self.admissible_heuristic(neighbor, agent_name)
+                f_score[neighbor] = g_score[neighbor] + self.env.admissible_heuristic(neighbor, agent_name)
         return False
 

centralized/cbs/cbs.py

@@ -14,6 +14,8 @@
 from copy import deepcopy
 
 from cbs.a_star import AStar
+from cbs.constraints import Constraints, EdgeConstraint, VertexConstraint
+
 
 class Location(object):
     def __init__(self, x=-1, y=-1):
@@ -54,43 +56,6 @@ def __str__(self):
         return '(' + str(self.time) + ', ' + self.agent_1 + ', ' + self.agent_2 + \
              ', '+ str(self.location_1) + ', ' + str(self.location_2) + ')'
 
-class VertexConstraint(object):
-    def __init__(self, time, location):
-        self.time = time
-        self.location = location
-
-    def __eq__(self, other):
-        return self.time == other.time and self.location == other.location
-    def __hash__(self):
-        return hash(str(self.time)+str(self.location))
-    def __str__(self):
-        return '(' + str(self.time) + ', '+ str(self.location) + ')'
-
-class EdgeConstraint(object):
-    def __init__(self, time, location_1, location_2):
-        self.time = time
-        self.location_1 = location_1
-        self.location_2 = location_2
-    def __eq__(self, other):
-        return self.time == other.time and self.location_1 == other.location_1 \
-            and self.location_2 == other.location_2
-    def __hash__(self):
-        return hash(str(self.time) + str(self.location_1) + str(self.location_2))
-    def __str__(self):
-        return '(' + str(self.time) + ', '+ str(self.location_1) +', '+ str(self.location_2) + ')'
-
-class Constraints(object):
-    def __init__(self):
-        self.vertex_constraints = set()
-        self.edge_constraints = set()
-
-    def add_constraint(self, other):
-        self.vertex_constraints |= other.vertex_constraints
-        self.edge_constraints |= other.edge_constraints
-
-    def __str__(self):
-        return "VC: " + str([str(vc) for vc in self.vertex_constraints])  + \
-            "EC: " + str([str(ec) for ec in self.edge_constraints])
 
 class Environment(object):
     def __init__(self, dimension, agents, obstacles):

centralized/cbs/constraints.py

@@ -0,0 +1,59 @@
+class Constraints(object):
+    def __init__(self):
+        self.vertex_constraints = set()
+        self.edge_constraints = set()
+
+    def add_constraint(self, other):
+        self.vertex_constraints |= other.vertex_constraints
+        self.edge_constraints |= other.edge_constraints
+
+    def __str__(self):
+        return (
+            "VC: "
+            + str([str(vc) for vc in self.vertex_constraints])
+            + "EC: "
+            + str([str(ec) for ec in self.edge_constraints])
+        )
+
+
+class VertexConstraint(object):
+    def __init__(self, time, location):
+        self.time = time
+        self.location = location
+
+    def __eq__(self, other):
+        return self.time == other.time and self.location == other.location
+
+    def __hash__(self):
+        return hash(str(self.time) + str(self.location))
+
+    def __str__(self):
+        return "(" + str(self.time) + ", " + str(self.location) + ")"
+
+
+class EdgeConstraint(object):
+    def __init__(self, time, location_1, location_2):
+        self.time = time
+        self.location_1 = location_1
+        self.location_2 = location_2
+
+    def __eq__(self, other):
+        return (
+            self.time == other.time
+            and self.location_1 == other.location_1
+            and self.location_2 == other.location_2
+        )
+
+    def __hash__(self):
+        return hash(str(self.time) + str(self.location_1) + str(self.location_2))
+
+    def __str__(self):
+        return (
+            "("
+            + str(self.time)
+            + ", "
+            + str(self.location_1)
+            + ", "
+            + str(self.location_2)
+            + ")"
+        )

centralized/test/test_cbs.py

@@ -0,0 +1,41 @@
+import os
+import sys
+
+import pytest
+sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
+
+from cbs.cbs import Environment, CBS
+
+@pytest.mark.timeout(5)  # Timeout after 5 seconds
+def test_cbs_two_agents():
+
+    """
+    X X O X X
+    a b O B A
+
+    a, b: agent starts
+    A, B: agent goals
+    X: obstacle
+    O: space
+    """
+    dimension = [5, 2]
+    obstacles = [(0, 1), (1,1), (3,1), (4,1)]
+
+    agents = [
+        {"start": [0, 0], "goal": [4, 0], "name": "A"},
+        {"start": [1, 0], "goal": [3, 0], "name": "B"},
+    ]
+
+
+    env = Environment(dimension, agents, obstacles)
+    cbs = CBS(env)
+
+    solution = cbs.search()
+
+    assert solution is not None
+
+    assert solution["A"][-1]["x"] == 4
+    assert solution["A"][-1]["y"] == 0
+
+    assert solution["B"][-1]["x"] == 3
+    assert solution["B"][-1]["y"] == 0

requirements.txt

@@ -1,4 +1,8 @@
+lark
 numpy
 matplotlib
 scipy
 ffmpeg-python
+pyyaml
+pytest
+pytest-timeout