-
Notifications
You must be signed in to change notification settings - Fork 6
/
prioritized.py
82 lines (69 loc) · 3.43 KB
/
prioritized.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
import time as timer
from single_agent_planner import compute_heuristics, a_star, get_sum_of_cost
class PrioritizedPlanningSolver(object):
"""A planner that plans for each robot sequentially."""
def __init__(self, my_map, starts, goals, max_time=None):
"""my_map - list of lists specifying obstacle positions
starts - [(x1, y1), (x2, y2), ...] list of start locations
goals - [(x1, y1), (x2, y2), ...] list of goal locations
"""
self.my_map = my_map
self.starts = starts
self.goals = goals
self.num_of_agents = len(goals)
self.max_time = max_time if max_time else float('inf')
self.num_of_expanded = 0
self.num_of_generated = 0
self.CPU_time = 0
# compute heuristics for the low-level search
self.heuristics = []
for goal in self.goals:
self.heuristics.append(compute_heuristics(my_map, goal))
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
constraints = []
for i in range(self.num_of_agents): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints)
if timer.time() - start_time >= self.max_time:
raise BaseException("Time limit exceeded")
if path is None:
raise BaseException('No solutions')
result.append(path)
##############################
# Task 2: Add constraints here
# Useful variables:
# * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)]
# * self.num_of_agents has the number of total agents
# * constraints: array of constraints to consider for future A* searches
##############################
for time, loc in enumerate(path):
# create a new constraint whith the current path location for all agents except the current one
for a in range(self.num_of_agents):
if a != i:
# vertex constraint
# if this is the last location in the path, we add a final constraint
constraints.append({
'agent': a,
'loc': [loc],
'timestep': time,
'final': time == len(path) - 1
})
# edge constraint
# the agent can't be at the last position to add an edge constraint
if time < len(path) - 1:
# next location in path
nextloc = path[path.index(loc) + 1]
constraints.append({
'agent': a,
'loc': [nextloc, loc],
'timestep': time + 1,
'final': False
})
self.CPU_time = timer.time() - start_time
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
print(result)
return result