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