def solve_problem(self, problem_instance):
self._reservation_table = dict()
self._completed_pos = []
paths = []
for i, agent in enumerate(problem_instance.get_agents()):
print(i, " ", agent)
if self._stop_event.is_set():
break
solver = AStar(self._solver_settings)
path = solver.find_path_with_reservation_table(
problem_instance.get_map(), agent.get_start(),
agent.get_goal(), self._reservation_table, self._completed_pos)
if not path:
paths.append([])
path = [agent.get_start()]
else:
paths.append(path)
for j, pos in enumerate(path):
if not self._reservation_table.get(pos):
self._reservation_table[pos] = []
self._reservation_table[pos].append(j)
self._reservation_table[pos].sort()
self._completed_pos.append(path[-1])
print("Path:", path)
print("Reservation table:", self._reservation_table)
print("Completed pos:", self._completed_pos)
self._solution = paths
def get_next_optimal_state(self):
from MAPFSolver.Utilities.AStar import AStar
from MAPFSolver.Utilities.SolverSettings import SolverSettings
solver = AStar(SolverSettings())
path = solver.find_path(self._map, self._position, self._goal)
next_pos = path[1]
return SingleAgentState(self._map,
self._goal,
next_pos,
self._solver_settings,
parent=self)
def get_next_optimal_state(self):
"""
Compute the next optimal state following the optimal policy.
:return: the next state following the optimal policy
"""
from MAPFSolver.Utilities.AStar import AStar
from MAPFSolver.Utilities.SolverSettings import SolverSettings
solver = AStar(SolverSettings())
path = solver.find_path(self._map, self._position, self._goal)
next_pos = path[1]
return SingleAgentState(self._map,
self._goal,
next_pos,
self._solver_settings,
parent=self)
def compute_optimal_costs_vector(self):
"""
Returns the the optimal costs vector. It will have all the optimal costs for each agent.
"""
path_costs_vector = []
solver = AStar(self._solver_settings)
for agent in self._problem_instance.get_agents():
path = solver.find_path(self._problem_instance.get_map(),
agent.get_start(), agent.get_goal())
if self._solver_settings.stay_at_goal():
cost = len(path) - 1
else:
cost = len(
path) - self._solver_settings.get_goal_occupation_time()
path_costs_vector.append(cost)
return path_costs_vector
def single_agent_low_level_search(self, agent):
agent_vertex_constraints = []
for vertex_constraint in self._vertex_constraints:
agent_id, pos, ts = vertex_constraint
if agent_id == agent.get_id():
agent_vertex_constraints.append((pos, ts))
agent_edge_constraints = []
for edge_constraint in self._edge_constraints:
agent_id, pos_i, pos_f, ts = edge_constraint
if agent_id == agent.get_id():
agent_edge_constraints.append((pos_i, pos_f, ts))
solver = AStar(self._solver_settings)
path = solver.find_path_with_constraints(
self._problem_instance.get_map(), agent.get_start(),
agent.get_goal(), agent_vertex_constraints, agent_edge_constraints)
return path
def solve_problem(self, problem_instance, verbose=False):
"""
Solve the given MAPF problem using the Cooperative A* algorithm.
:param problem_instance: instance of the problem to solve.
:param verbose: if True, infos will be printed on terminal.
"""
self._reservation_table = dict()
self._completed_pos = []
paths = []
for i, agent in enumerate(problem_instance.get_agents()):
if self._stop_event.is_set():
break
if verbose:
print("Agent n:", i, "of", len(problem_instance.get_agents()))
solver = AStar(self._solver_settings)
path = solver.find_path_with_reservation_table(
problem_instance.get_map(), agent.get_start(),
agent.get_goal(), self._reservation_table, self._completed_pos)
if not path:
paths.append([])
path = [agent.get_start()]
else:
paths.append(path)
for j, pos in enumerate(path):
if not self._reservation_table.get(pos):
self._reservation_table[pos] = []
self._reservation_table[pos].append(j)
self._reservation_table[pos].sort()
if self._solver_settings.stay_at_goal():
self._completed_pos.append(path[-1])
"""print("Path:", path)
print("Reservation table:", self._reservation_table)
print("Completed pos:", self._completed_pos)"""
self._solution = paths
def single_agent_low_level_search(self, agent):
"""
Low level search for a single agent. It searches a possible valid path using A* which doesn't violate the set
of constraints.
"""
agent_vertex_constraints = []
for vertex_constraint in self._vertex_constraints:
agent_id, pos, ts = vertex_constraint
if agent_id == agent.get_id():
agent_vertex_constraints.append((pos, ts))
agent_edge_constraints = []
for edge_constraint in self._edge_constraints:
agent_id, pos_i, pos_f, ts = edge_constraint
if agent_id == agent.get_id():
agent_edge_constraints.append((pos_i, pos_f, ts))
solver = AStar(self._solver_settings)
path = solver.find_path_with_constraints(
self._problem_instance.get_map(), agent.get_start(),
agent.get_goal(), agent_vertex_constraints, agent_edge_constraints)
return path