diff --git a/Algorithms/Centralized/Suboptimal_CBS/EnvChannel.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/EnvChannel.py
similarity index 100%
rename from Algorithms/Centralized/Suboptimal_CBS/EnvChannel.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/EnvChannel.py
diff --git a/Algorithms/Centralized/Suboptimal_CBS/Greedy_CBS/decisionmaker_shortest_path.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/Greedy_CBS/decisionmaker_greedy_cbs.py
similarity index 84%
rename from Algorithms/Centralized/Suboptimal_CBS/Greedy_CBS/decisionmaker_shortest_path.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/Greedy_CBS/decisionmaker_greedy_cbs.py
index 632aa4daa41fc850f81aad7dfb562359d9aeb382..daeca98c7c63696ea9e43c8b155c16c8697e4917 100644
--- a/Algorithms/Centralized/Suboptimal_CBS/Greedy_CBS/decisionmaker_shortest_path.py
+++ b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/Greedy_CBS/decisionmaker_greedy_cbs.py
@@ -1,10 +1,11 @@
from mlagents_envs.environment import ActionTuple, UnityEnvironment
-import EnvChannel as EC
-from cbs_mapf.planner import Planner as planner
-from util import *
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.cbs_mapf.planner import Planner as planner
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.util import *
import numpy as np
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS import EnvChannel as EC
+
if __name__ == '__main__':
# as long as simulation_going is true, new actions will be generated:
@@ -44,10 +45,10 @@ if __name__ == '__main__':
robot_ends_array[i]["GridPosition"]["Column"],
robot_ends_array[i]["Degrees"]])
- static_obstacles, highways = environmentToTupleList(environment_matrix) # All static obstacles (walls) positions in a list
- cbsplanner = planner(static_obstacles, rotation, robot_can_go_backwards)
- shortest_paths = cbsplanner.plan(starts, goals).tolist()
- #print(shortest_paths)
+ static_obstacles = environmentToTupleList(environment_matrix) # All static obstacles (walls) positions in a list
+ greedy_cbs_planner = planner(static_obstacles, rotation, robot_can_go_backwards)
+ solution_paths = greedy_cbs_planner.plan(starts, goals).tolist()
+ #print(solution_paths)
while simulation_going:
# get information from the agent that requests an action:
@@ -65,7 +66,7 @@ if __name__ == '__main__':
#print("robot " + str(i) + " at node [(" + str(observation[0]) + "," + str(observation[1]) + ")," + str(
#observation[2]) + "]")
- path = shortest_paths[i]
+ path = solution_paths[i]
# if path is empty, then robot should stay at same node:
if len(path) == 0:
@@ -74,7 +75,7 @@ if __name__ == '__main__':
else:
next_action = path[0] # is a node
actions.extend([observation[0], next_action[0], next_action[1], next_action[2]])
- shortest_paths[i] = path[1:]
+ solution_paths[i] = path[1:]
# add action to action_tuple and set the actions:
action_tuple.add_discrete(np.array([actions]))
diff --git a/Algorithms/Centralized/Algorithm_Suboptimal_CBS/Greedy_CBS/greedy_constraint_tree.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/Greedy_CBS/greedy_constraint_tree.py
new file mode 100644
index 0000000000000000000000000000000000000000..060d5df29a5538793aa6f253f5bdf360701c73a5
--- /dev/null
+++ b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/Greedy_CBS/greedy_constraint_tree.py
@@ -0,0 +1,19 @@
+from typing import Dict
+
+import numpy as np
+
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.cbs_mapf.agent import Agent
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.cbs_mapf.constraint_tree import CTNode
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.cbs_mapf.constraints import Constraints
+
+
+class GreedyCTNode(CTNode):
+
+ def __init__(self, constraints: Constraints,
+ solution: Dict[Agent, np.ndarray],
+ conflicts):
+ super().__init__(constraints, solution)
+ self.conflicts = conflicts
+
+ def __lt__(self, other):
+ return self.conflicts < other.conflicts
diff --git a/Algorithms/Centralized/Suboptimal_CBS/Pipfile b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/Pipfile
similarity index 100%
rename from Algorithms/Centralized/Suboptimal_CBS/Pipfile
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/Pipfile
diff --git a/Algorithms/Centralized/Suboptimal_CBS/Pipfile.lock b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/Pipfile.lock
similarity index 100%
rename from Algorithms/Centralized/Suboptimal_CBS/Pipfile.lock
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/Pipfile.lock
diff --git a/Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/__init__.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/__init__.py
similarity index 100%
rename from Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/__init__.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/__init__.py
diff --git a/Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/agent.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/agent.py
similarity index 100%
rename from Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/agent.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/agent.py
diff --git a/Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/assigner.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/assigner.py
similarity index 96%
rename from Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/assigner.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/assigner.py
index ba5ecad1a0b8c2475192f7da75e5d93504fda242..75531f15e51905e8c38cafd3467012bd54099037 100644
--- a/Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/assigner.py
+++ b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/assigner.py
@@ -12,7 +12,7 @@ from typing import List, Tuple
import numpy as np
from scipy.optimize import linear_sum_assignment
-from cbs_mapf.agent import Agent
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.cbs_mapf.agent import Agent
def standard(starts: List[Tuple[int, int, int]], goals: List[Tuple[int, int, int]]):
diff --git a/Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/constraint_tree.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/constraint_tree.py
similarity index 81%
rename from Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/constraint_tree.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/constraint_tree.py
index 28b15892c8f40726d1c0a59cce97bd4fd09b5882..2c77249779f92ad91f440246a921a74d40232e03 100644
--- a/Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/constraint_tree.py
+++ b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/constraint_tree.py
@@ -11,8 +11,8 @@ An implementation of multi-agent path finding using conflict-based search
from typing import Dict
import numpy as np
-from cbs_mapf.agent import Agent
-from cbs_mapf.constraints import Constraints
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.cbs_mapf.agent import Agent
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.cbs_mapf.constraints import Constraints
class CTNode:
diff --git a/Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/constraints.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/constraints.py
similarity index 94%
rename from Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/constraints.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/constraints.py
index 4ae8a62a870e5936efcb3fe55e0fa39355bc1516..12991b3fc7ccc0f7da2eb1e3be4f2e69cd97fd7e 100644
--- a/Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/constraints.py
+++ b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/constraints.py
@@ -11,7 +11,7 @@ An implementation of multi-agent path finding using conflict-based search
from copy import deepcopy
from typing import Dict, Tuple, List
-from cbs_mapf.agent import Agent
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.cbs_mapf.agent import Agent
'''
Emulated dictionary of dictionaries
diff --git a/Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/planner.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/planner.py
similarity index 83%
rename from Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/planner.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/planner.py
index a45efa09376c936b08e1352ec2297f1eda86ef79..7c8bccddb0f8e72dbf9045f02845b658057354ca 100644
--- a/Algorithms/Centralized/Suboptimal_CBS/cbs_mapf/planner.py
+++ b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/cbs_mapf/planner.py
@@ -17,14 +17,18 @@ from itertools import combinations
from typing import Dict, Callable
import numpy as np
-from cbs_mapf.assigner import *
-from cbs_mapf.constraint_tree import CTNode
-from cbs_mapf.constraints import Constraints
+
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.Greedy_CBS.greedy_constraint_tree import GreedyCTNode
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.cbs_mapf.assigner import *
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.cbs_mapf.constraint_tree import CTNode
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.cbs_mapf.constraints import Constraints
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.conflicts_heuristic import ConflictsHeuristic
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.util import distance
# The low level planner for CBS is the Space-Time A* planner
# https://github.com/GavinPHR/Space-Time-AStar
-from stastar.planner import Planner as STPlanner
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.stastar.planner import Planner as STPlanner
def calculate_goal_times(node: CTNode, agent: Agent, agents: List[Agent]):
@@ -64,6 +68,7 @@ class Planner:
self.robot_radius = 0.5
self.st_planner = STPlanner(rotation, 1, 0.5, static_obstacles, robot_can_go_backwards)
+ self.conflicts_heuristic = ConflictsHeuristic(self.safe_distance)
'''
You can use your own assignment function, 'standard' will use the order of the config file,
@@ -92,7 +97,8 @@ class Planner:
visited = []
if all(len(path) != 0 for path in solution.values()):
# Make root node:
- node = CTNode(constraints, solution)
+ node = GreedyCTNode(constraints, solution, self.conflicts_heuristic.
+ round_robin_conflicts_heuristic(solution, self.agents, self.robot_radius))
# Min heap for quick extraction:
open.append(node)
@@ -158,11 +164,13 @@ class Planner:
node_i = None
if all(len(path) != 0 for path in solution_i.values()): # When a valid solution is found
- node_i = CTNode(agent_i_constraints, solution_i)
+ node_i = GreedyCTNode(agent_i_constraints, solution_i, self.conflicts_heuristic.
+ round_robin_conflicts_heuristic(solution_i, self.agents, self.robot_radius))
node_j = None
if all(len(path) != 0 for path in solution_j.values()):
- node_j = CTNode(agent_j_constraints, solution_j)
+ node_j = GreedyCTNode(agent_j_constraints, solution_j, self.conflicts_heuristic.
+ round_robin_conflicts_heuristic(solution_j, self.agents, self.robot_radius))
results.append((node_i, node_j))
@@ -174,37 +182,33 @@ class Planner:
# Check collision pair-wise
combs = list(combinations(agents, 2))
for agent_i, agent_j in combs:
- time_of_conflict = self.safe_distance(node.solution, agent_i, agent_j)
+ time_of_conflict, collision_type = self.safe_distance(node.solution, agent_i, agent_j, self.robot_radius)
if time_of_conflict == -1:
continue
return agent_i, agent_j, time_of_conflict
return None, None, -1
- def safe_distance(self, solution: Dict[Agent, np.ndarray], agent_i: Agent, agent_j: Agent) -> int:
+ def safe_distance(self, solution: Dict[Agent, np.ndarray], agent_i: Agent, agent_j: Agent, robot_radius):
max_sol = max(len(solution[agent_i]), len(solution[agent_j]))
# make solutions of agent_i and agent_j the same length by extending the list by duplicating the last item:
sol1, sol2 = extend_paths(solution[agent_i].copy(), solution[agent_j].copy(), max_sol)
# check for each timestep if the two agents collide:
for timestep, (point_i, point_j) in enumerate(zip(sol1, sol2)):
# check agents being in a safe distance from each other:
- if self.distance(point_i, point_j) >= 2 * self.robot_radius:
+ if distance(point_i, point_j) >= 2 * robot_radius:
if timestep < (min(len(solution[agent_i]), len(solution[agent_j])) - 1) and (
# Check for train collisions
(solution[agent_i][timestep][:2] == solution[agent_j][timestep + 1][:2]).all() or
(solution[agent_j][timestep][:2] == solution[agent_i][timestep + 1][:2]).all()):
- return timestep + 1
+ return timestep + 1, "train_collision"
else:
continue
else:
- return timestep
- return -1
+ return timestep, "normal_collision"
+ return -1, "no_collision"
@staticmethod
- def distance(position1: np.ndarray, position2: np.ndarray) -> int:
- return np.abs(position1[0] - position2[0]) + np.abs(
- position1[1] - position2[1])
-
- def calculate_constraints(self, node: CTNode, # returns updated constraints
+ def calculate_constraints(node: CTNode, # returns updated constraints
constrained_agent: Agent,
time_of_conflict: int) -> Constraints:
constrained_path = node.solution[constrained_agent]
diff --git a/Algorithms/Centralized/Suboptimal_CBS/codeStyles/Project.xml b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/codeStyles/Project.xml
similarity index 100%
rename from Algorithms/Centralized/Suboptimal_CBS/codeStyles/Project.xml
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/codeStyles/Project.xml
diff --git a/Algorithms/Centralized/Suboptimal_CBS/codeStyles/codeStyleConfig.xml b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/codeStyles/codeStyleConfig.xml
similarity index 100%
rename from Algorithms/Centralized/Suboptimal_CBS/codeStyles/codeStyleConfig.xml
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/codeStyles/codeStyleConfig.xml
diff --git a/Algorithms/Centralized/Suboptimal_CBS/conflicts_heuristic.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/conflicts_heuristic.py
similarity index 71%
rename from Algorithms/Centralized/Suboptimal_CBS/conflicts_heuristic.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/conflicts_heuristic.py
index 139928752f5a57a3798c0075b2a3bb236978171a..015a7a20d320fd2fa3803b533a8a9b34733d7f03 100644
--- a/Algorithms/Centralized/Suboptimal_CBS/conflicts_heuristic.py
+++ b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/conflicts_heuristic.py
@@ -1,21 +1,27 @@
from itertools import combinations
+from typing import Dict, List
+
+import numpy as np
+
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.cbs_mapf.agent import Agent
'''
Conflicts heuristics
'''
-class Conflicts_heuristic:
+class ConflictsHeuristic:
- def __init__(self):
+ def __init__(self, safe_distance_method):
self.round_robin_ctr = 0
+ self.safe_distance_method = safe_distance_method
- def number_of_conflicts(self, solution: Dict[Agent, np.ndarray], agents: List[Agent]) -> int:
+ def number_of_conflicts(self, solution: Dict[Agent, np.ndarray], agents: List[Agent], robot_radius) -> int:
conflicts = 0
# Check collision pair-wise
combs = list(combinations(agents, 2))
for agent_i, agent_j in combs:
- time_of_conflict, collision_type = self.safe_distance(solution, agent_i, agent_j)
+ time_of_conflict, collision_type = self.safe_distance_method(solution, agent_i, agent_j, robot_radius)
updated_solution = solution.copy() # update solution paths to find collisions after a first collision got found
while time_of_conflict != -1:
conflicts += 1
@@ -23,36 +29,36 @@ class Conflicts_heuristic:
time_of_conflict += 1
updated_solution[agent_i] = updated_solution[agent_i][time_of_conflict:]
updated_solution[agent_j] = updated_solution[agent_j][time_of_conflict:]
- time_of_conflict, collision_type = self.safe_distance(updated_solution, agent_i, agent_j)
+ time_of_conflict, collision_type = self.safe_distance_method(updated_solution, agent_i, agent_j, robot_radius)
return conflicts
- def number_of_conflicting_agents(self, solution: Dict[Agent, np.ndarray], agents: List[Agent]) -> int:
+ def number_of_conflicting_agents(self, solution: Dict[Agent, np.ndarray], agents: List[Agent], robot_radius) -> int:
conflicts = 0
for agent_i in agents:
for agent_j in agents:
if agent_i != agent_j:
- time_of_conflict, collision_type = self.safe_distance(solution, agent_i, agent_j)
+ time_of_conflict, collision_type = self.safe_distance_method(solution, agent_i, agent_j, robot_radius)
if time_of_conflict != -1:
conflicts += 1
break
return conflicts
- def number_of_conflicting_pairs(self, solution: Dict[Agent, np.ndarray], agents: List[Agent]) -> int:
+ def number_of_conflicting_pairs(self, solution: Dict[Agent, np.ndarray], agents: List[Agent], robot_radius) -> int:
conflicts = 0
# Check collision pair-wise
combs = list(combinations(agents, 2))
for agent_i, agent_j in combs:
- time_of_conflict, collision_type = self.safe_distance(solution, agent_i, agent_j)
+ time_of_conflict, collision_type = self.safe_distance_method(solution, agent_i, agent_j, robot_radius)
if time_of_conflict != -1:
conflicts += 1
return conflicts
- def round_robin_conflicts_heuristic(self, solution: Dict[Agent, np.ndarray], agents: List[Agent]) -> int:
+ def round_robin_conflicts_heuristic(self, solution: Dict[Agent, np.ndarray], agents: List[Agent], robot_radius) -> int:
round_robin_decision = self.round_robin_ctr % 3
self.round_robin_ctr += 1
if round_robin_decision == 0:
- return self.number_of_conflicts(solution, agents)
+ return self.number_of_conflicts(solution, agents, robot_radius)
elif round_robin_decision == 1:
- return self.number_of_conflicting_agents(solution, agents)
+ return self.number_of_conflicting_agents(solution, agents, robot_radius)
else: # round_robin_decision == 2
- return self.number_of_conflicting_pairs(solution, agents)
+ return self.number_of_conflicting_pairs(solution, agents, robot_radius)
diff --git a/Algorithms/Centralized/Suboptimal_CBS/readme.md b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/readme.md
similarity index 96%
rename from Algorithms/Centralized/Suboptimal_CBS/readme.md
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/readme.md
index 80048ded11d0dcb79f3cee743861ac3d5256c202..4211cd26c08333d304df3c5df8d438652f5642e4 100644
--- a/Algorithms/Centralized/Suboptimal_CBS/readme.md
+++ b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/readme.md
@@ -21,6 +21,12 @@ Algorithm implemented by Elias De Deken, see Bachelor Thesis for pseudo code and
- rotation has to be a factor(divisor) of 90 degrees
- the InitialNodes degrees of each agent has to be a rotation of 0, 90, 180 or 270
+##
+## main files
+##
+for every suboptimal CBS variant there is a directory with a decisionmaker_<Suboptimal_CBS_variant>.py file. To run the
+suboptimal variant of CBS you need to run this file.
+
##
## cbs_mapf
##
diff --git a/Algorithms/Centralized/Suboptimal_CBS/stastar/__init__.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/stastar/__init__.py
similarity index 100%
rename from Algorithms/Centralized/Suboptimal_CBS/stastar/__init__.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/stastar/__init__.py
diff --git a/Algorithms/Centralized/Suboptimal_CBS/stastar/grid.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/stastar/grid.py
similarity index 100%
rename from Algorithms/Centralized/Suboptimal_CBS/stastar/grid.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/stastar/grid.py
diff --git a/Algorithms/Centralized/Suboptimal_CBS/stastar/neighbour_table.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/stastar/neighbour_table.py
similarity index 100%
rename from Algorithms/Centralized/Suboptimal_CBS/stastar/neighbour_table.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/stastar/neighbour_table.py
diff --git a/Algorithms/Centralized/Suboptimal_CBS/stastar/planner.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/stastar/planner.py
similarity index 97%
rename from Algorithms/Centralized/Suboptimal_CBS/stastar/planner.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/stastar/planner.py
index 516820b52cc69485d52562893bdb764dce013c1c..21049635a80c86651b59e5da2603e23ca5b333b0 100644
--- a/Algorithms/Centralized/Suboptimal_CBS/stastar/planner.py
+++ b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/stastar/planner.py
@@ -16,9 +16,9 @@ from math import floor, ceil
import numpy as np
from scipy.spatial import KDTree
-from stastar.grid import Grid
-from stastar.neighbour_table import NeighbourTable
-from stastar.state import State
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.stastar.grid import Grid
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.stastar.neighbour_table import NeighbourTable
+from Algorithms.Centralized.Algorithm_Suboptimal_CBS.stastar.state import State
def make_grid(static_obstacles) -> List[list]:
@@ -221,7 +221,7 @@ class Planner:
goal = np.asarray(goal)
# Initialize the start state:
- s = State(start, 0, 0, true_heuristics[start[0]][start[1]][int(round(start[2] / 90)) % 4])
+ s = State(start, 0, 0, true_heuristics[start[0]][start[1]][int(round(start[2] / 90)) % 4], 0)
open_set = [s]
closed_set = set()
diff --git a/Algorithms/Centralized/Suboptimal_CBS/stastar/state.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/stastar/state.py
similarity index 94%
rename from Algorithms/Centralized/Suboptimal_CBS/stastar/state.py
rename to Algorithms/Centralized/Algorithm_Suboptimal_CBS/stastar/state.py
index 1c302c2b57958c375060bad63c209aa3333bf5ce..02f37cff2931f35e355dae0cc78b2793e8ffa8f9 100644
--- a/Algorithms/Centralized/Suboptimal_CBS/stastar/state.py
+++ b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/stastar/state.py
@@ -13,11 +13,13 @@ import numpy as np
class State:
- def __init__(self, pos: np.ndarray, time: int, g_score: int, h_score: int):
+ def __init__(self, pos: np.ndarray, time: int, g_score: int, h_score: int, conflicts: int):
self.pos = pos
self.time = time
self.g_score = g_score
self.f_score = g_score + h_score
+ self.conflicts = conflicts
+
def __hash__(self) -> int:
return int(str(self.pos[0]) + '3' + str(self.pos[2]) + '123456789' + str(self.pos[1]) + '3' + str(self.time))
diff --git a/Algorithms/Centralized/Algorithm_Suboptimal_CBS/util.py b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/util.py
new file mode 100644
index 0000000000000000000000000000000000000000..e734c9854daecdebb49f7736d959b2ecef2c7de6
--- /dev/null
+++ b/Algorithms/Centralized/Algorithm_Suboptimal_CBS/util.py
@@ -0,0 +1,16 @@
+import numpy as np
+
+
+def environmentToTupleList(matrix):
+ static_obstacles = list()
+ for row in range(len(matrix)):
+ for column in range(len(matrix[row])):
+ element = matrix[row][column]
+ if element == 1:
+ position = tuple([row, column])
+ static_obstacles.append(position)
+ return static_obstacles
+
+
+def distance(position1: np.ndarray, position2: np.ndarray) -> int:
+ return np.abs(position1[0] - position2[0]) + np.abs(position1[1] - position2[1])
diff --git a/Algorithms/Centralized/Suboptimal_CBS/util.py b/Algorithms/Centralized/Suboptimal_CBS/util.py
deleted file mode 100644
index a663f5f0c1d7ca9b04cf543d58cad24e754de318..0000000000000000000000000000000000000000
--- a/Algorithms/Centralized/Suboptimal_CBS/util.py
+++ /dev/null
@@ -1,19 +0,0 @@
-def environmentToTupleList(matrix):
- static_obstacles = list()
- highways = []
- for row in range(len(matrix)):
- for column in range(len(matrix[row])):
- element = matrix[row][column]
- if element == 1:
- position = tuple([row, column])
- static_obstacles.append(position)
- if element == 6:
- highways.append(tuple([row, column, 0]))
- if element == 3:
- highways.append(tuple([row, column, 180]))
- if element == 4:
- highways.append(tuple([row, column, 90]))
- if element == 5:
- highways.append(tuple([row, column, 270]))
- return static_obstacles, highways
-