def __init__(self, world, start, goal):
self.world = world
self.start = start
self.goal = goal
self.h = RRAstar(world, start, goal)
self.path = []
self.conflicts = SortedListWithKey(key=lambda c: c.time)
self.priorities = []
self.stable_prio = []
self._actual_prio = set()
self.old_conflicts = set()
def __init__(self, world, start, goal,
weights=Weights(1, 3), caching=True):
self.world = world
self.start = start
self.goal = goal
self.h = RRAstar(world, start, goal)
self.path = []
self.path_cache = {}
self.conflicts = SortedListWithKey(key=lambda conflict: conflict.time)
self.resolved_conflicts = []
self.current_conflict = None
self.higher_prio = frozenset()
self.weights = weights
self.caching = caching
def __init__(self, starts, goals, w):
self.starts = starts
self.goals = goals
self.paths = []
self.heur = {}
for i in range(len(starts)):
self.heur[goals[i]] = RRAstar(w, starts[i], goals[i])
def astar(world, start, goal, window):
closed_set = set()
open_set = []
came_from = {}
g = {start: 0}
heapq.heappush(open_set, (0, 0, start))
h = RRAstar(world, start, goal)
while open_set:
f, time_step, cur = heapq.heappop(open_set)
print('evaluating', f, time_step, cur)
pprint(open_set)
if time_step > window:
return reverse_path((time_step, cur), came_from)
closed_set.add(cur)
for successor in (world.neighbours(cur) + [cur]):
if successor in closed_set and cur != successor:
continue
if time_step == window:
score = g[cur] + h.dist(successor)
elif cur == goal and successor == goal:
score = g[cur]
else:
score = g[cur] + 1
# Ignore a path if it is a longer variant
if successor in g and score >= g[successor] and successor != cur:
continue
came_from[time_step + 1, successor] = (time_step, cur)
g[successor] = score
if time_step == window:
heapq.heappush(open_set, (score, time_step + 1, successor))
else:
heapq.heappush(
open_set,
(score + h.dist(successor), time_step + 1, successor))
raise util.NoPathsFoundException()
def standard_algorithm(agents, world, starts, goals, start_time=None,
max_time=None):
starts = tuple(starts)
goals = tuple(goals)
closed_set = set()
open_set = []
came_from = {}
g = {starts: 0}
count = 0
heapq.heappush(open_set, (0, count, starts))
# Set up heuristics
heur = {}
for i in range(agents):
heur[goals[i]] = RRAstar(world, starts[i], goals[i])
# Display predicted cost
pred_cost = heur_dist(heur, goals, starts)
print(f'predicted cost: {pred_cost}')
while open_set:
time = timeit.default_timer()
if start_time != None and (time - start_time) > max_time:
raise TimeExceeded()
f, _, current = heapq.heappop(open_set)
#print(f'f: {f:4}, current: {current}')
if current == goals:
return reconstruct_path(came_from, current)
closed_set.add(current)
for cost, neighbour in successor_states(world, current, goals,
start_time, max_time):
if neighbour in closed_set:
continue
score = g[current] + cost
# We found a longer path, ignore it
if neighbour in g and score >= g[neighbour]:
continue
came_from[neighbour] = current
g[neighbour] = score
count += 1
heapq.heappush(open_set, (score + heur_dist(heur, goals, current),
count, neighbour))
return None
def __init__(self, world, start, goal):
self.world = world
self.start = start
self.goal = goal
self.h = RRAstar(world, start, goal)
self.path = []
class Agent:
def __init__(self, world, start, goal):
self.world = world
self.start = start
self.goal = goal
self.h = RRAstar(world, start, goal)
self.path = []
def plan(self, global_plan, start_time, max_time):
self.path = self._astar(global_plan, start_time, max_time)
def _astar(self, global_plan, start_time, max_time):
closed_set = set()
open_set = []
came_from = {}
g = {self.start: 0}
heapq.heappush(open_set, (0, 0, self.start))
while open_set:
time = timeit.default_timer()
if start_time != None and (time - start_time) > max_time:
raise TimeExceeded()
_, time_step, cur = heapq.heappop(open_set)
if cur == self.goal:
return self._reverse_path((time_step, cur), came_from)
closed_set.add(cur)
for successor in self._successors(cur, time_step, global_plan,
start_time, max_time):
# Skip successor in closed list
if successor in closed_set and successor != cur:
continue
score = g[cur] + 1
# Ignore path if it is a longer variant
if successor in g and score >= g[successor] \
and successor != cur:
continue
came_from[time_step + 1, successor] = (time_step, cur)
g[successor] = score
heapq.heappush(open_set,
(score + self.h.dist(successor), time_step + 1,
successor))
raise util.NoPathsFoundException()
def _successors(self, pos, time, global_plan, start_time, max_time):
successors = [pos] + self.world.neighbours(pos)
filtered = []
for successor in successors:
for other_path in global_plan:
cur_time = timeit.default_timer()
if start_time != None and (cur_time - start_time) > max_time:
raise TimeExceeded()
if len(other_path[time:]) >= 2:
if util.moves_conflict(other_path[time:time + 2],
(pos, successor)):
break
elif util.moves_conflict((other_path[-1], other_path[-1]),
(pos, successor)):
break
else:
filtered.append(successor)
return filtered
def _reverse_path(self, state, came_from):
path = [state[1]]
while state in came_from:
state = came_from[state]
path.append(state[1])
path.reverse()
return path
def __repr__(self):
return f'{self.start}-{self.goal}'
class Agent:
def __init__(self, world, start, goal,
weights=Weights(1, 3), caching=True):
self.world = world
self.start = start
self.goal = goal
self.h = RRAstar(world, start, goal)
self.path = []
self.path_cache = {}
self.conflicts = SortedListWithKey(key=lambda conflict: conflict.time)
self.resolved_conflicts = []
self.current_conflict = None
self.higher_prio = frozenset()
self.weights = weights
self.caching = caching
def plan(self, start_time, max_time):
#print(f'Planning for agent {self}')
self.old_path = self.path
self.construct_higher_prio()
# Check if there is a path in the cache for this prio set up
if self.caching and self.higher_prio in self.path_cache:
# Check if the cached path conflicts with those of higher prio
paths = [agent.path for agent in self.higher_prio]
conflicts = util.paths_conflict(paths)
if not conflicts:
#print('Using cached path')
self.path = self.path_cache[self.higher_prio]
return
self.path = self._astar(start_time, max_time)
# Update the cache
self.path_cache[self.higher_prio] = self.path
def construct_higher_prio(self):
prio = []
# Construct priorities from the conflict solutions
for conflict in self.resolved_conflicts:
for i in range(conflict.solution['level'] + 1):
if conflict.solution[i] == self:
continue
prio.append(conflict.solution[i])
# Update with the current proposed solution
if self.current_conflict != None:
for i in range(self.current_conflict.proposal['level'] + 1):
if self.current_conflict.proposal[i] == self:
continue
prio.append(self.current_conflict.proposal[i])
self.higher_prio = frozenset(prio)
#print(f' Agent {self} final prio: {self.higher_prio}')
def propose(self, conflict):
# If there are no proposals yet, propose to go first
if len(conflict.proposals) == 0:
proposal = {'score': None, 'level': 0, 0: self}
return proposal
def resolved_conflict(self, conflict):
self.resolved_conflicts.append(conflict)
def evaluate(self, conflicts):
# Current conflict solved
if self.current_conflict in conflicts.values():
#print('Conflict not solved')
raise ConflictNotSolved()
score = 0
# Change in path length
score += (len(self.old_path) - len(self.path)) * self.weights.path_len
# Change in conflicts
filtered = list(filter(lambda c: self in c.agents, conflicts.values()))
#print(f'{self} {len(self.conflicts)} {len(filtered)}')
score += (len(self.conflicts) - len(filtered)) * \
self.weights.conflict_count
#print(f'Agent score {self}: {score}')
return score
def _astar(self, start_time, max_time):
closed_set = set()
open_set = []
came_from = {}
g = {self.start: 0}
heapq.heappush(open_set, (0, 0, self.start))
while open_set:
time = timeit.default_timer()
if start_time != None and (time - start_time) > max_time:
raise TimeExceeded()
_, time_step, cur = heapq.heappop(open_set)
if cur == self.goal:
return self._reverse_path((time_step, cur), came_from)
closed_set.add(cur)
for successor in self._successors(cur, time_step, start_time,
max_time):
# Skip successor in closed list
if successor in closed_set and successor != cur:
continue
score = g[cur] + 1
# Ignore a path if it is a longer variant
if successor in g and score >= g[successor] \
and successor != cur:
continue
came_from[time_step + 1, successor] = (time_step, cur)
g[successor] = score
heapq.heappush(open_set,
(score + self.h.dist(successor), time_step + 1,
successor))
raise util.NoPathsFoundException()
def _successors(self, pos, time, start_time, max_time):
successors = [pos] + self.world.neighbours(pos)
filtered = []
for successor in successors:
for other_agent in self.higher_prio:
cur_time = timeit.default_timer()
if start_time != None and (cur_time - start_time) > max_time:
raise TimeExceeded()
path = other_agent.path
if len(path[time:]) >= 2:
if util.moves_conflict(path[time:time + 2],
(pos, successor)):
break
else:
if util.moves_conflict((path[-1], path[-1]),
(pos, successor)):
break
else:
filtered.append(successor)
return filtered
def _reverse_path(self, state, came_from):
path = [state[1]]
while state in came_from:
state = came_from[state]
path.append(state[1])
path.reverse()
return path
def __repr__(self):
return f'{self.start}-{self.goal}'
def od(agents, w, starts, goals):
start_state = tuple(State(s) for s in starts)
count = 0
closed_set = set()
open_set = []
came_from = {}
g = {start_state: 0}
heapq.heappush(open_set, (0, count, 0, start_state))
# Set up heuristics
heur = {}
for i in range(agents):
heur[goals[i]] = RRAstar(w, starts[i], goals[i])
# Display predicted cost
pred_cost = heur_dist(heur, goals, starts)
print('predicted cost:', pred_cost)
while open_set:
f, _, agent, current = heapq.heappop(open_set)
#print(f'f: {f:4}, agent: {agent}, current: {current}')
# Check if we've reached the goal
if agent == 0:
state = tuple(s.pos for s in current)
if state == goals:
print('open set size: ', len(open_set))
print('closed set size:', len(closed_set))
print('final cost: ', f)
return reverse_paths(state, came_from)
# If we haven't then add the standard state to the closed set
closed_set.add(current)
# Add all possible actions
successors = next_states(current, w, agent)
for new_state in successors:
count += 1
# If the agent is in its goal position and the action is wait
# then there should be no cost
if new_state[agent].action == Actions.wait and \
new_state[agent].pos == goals[agent]:
score = g[current]
else:
score = g[current] + 1
# Create a standard state if necessary
if agent == agents - 1:
new_state = tuple(State(s.new_pos()) for s in new_state)
# Don't add it if it already exists
if new_state in closed_set:
continue
if new_state in g and score >= g[new_state]:
continue
# Check if the standard state is already in the open set
if new_state not in g:
# No duplicate found, add this one
simple_state = tuple(s.new_pos() for s in new_state)
h = heur_dist(heur, goals, simple_state)
heapq.heappush(open_set, (score + h, count, 0, new_state))
came_from[simple_state] = tuple(s.pos for s in current)
# Create intermediate state
else:
# If we found a longer path, ignore it
if new_state in g and score >= g[new_state]:
continue
h = 0
for i in range(agent + 1):
h += heur[goals[i]].dist(new_state[i].new_pos())
for i in range(agent + 1, agents):
h += heur[goals[i]].dist(new_state[i].pos)
heapq.heappush(open_set,
(score + h, count, agent + 1, new_state))
g[new_state] = score
return None
class Agent:
def __init__(self, world, start, goal):
self.world = world
self.start = start
self.goal = goal
self.h = RRAstar(world, start, goal)
self.path = []
self.conflicts = SortedListWithKey(key=lambda c: c.time)
self.priorities = []
self.stable_prio = []
self._actual_prio = set()
self.old_conflicts = set()
def plan(self, start_time=None, max_time=None):
self.path = self._astar(start_time, max_time)
def _astar(self, start_time, max_time):
self._actual_prio = set(self.stable_prio + self.priorities)
closed_set = set()
open_set = []
came_from = {}
g = {self.start: 0}
heapq.heappush(open_set, (0, 0, self.start))
while open_set:
time = timeit.default_timer()
if start_time != None and (time - start_time) > max_time:
raise TimeExceeded()
_, time_step, cur = heapq.heappop(open_set)
if cur == self.goal:
return self._reverse_path((time_step, cur), came_from)
closed_set.add(cur)
for successor in self._successors(cur, time_step, start_time,
max_time):
# Skip successor in closed list
if successor in closed_set and successor != cur:
continue
score = g[cur] + 1
# Ignore a path if it is longer
if successor in g and score >= g[successor] \
and successor != cur:
continue
came_from[time_step + 1, successor] = (time_step, cur)
g[successor] = score
heapq.heappush(
open_set,
(score + self.h.dist(successor), time_step + 1, successor))
raise util.NoPathsFoundException()
def _successors(self, pos, time, start_time=None, max_time=None):
successors = [pos] + self.world.neighbours(pos)
filtered = []
for successor in successors:
for other_agent in self._actual_prio:
cur_time = timeit.default_timer()
if start_time != None and (cur_time - start_time) > max_time:
raise TimeExceeded()
path = other_agent.path
if len(path[time:]) >= 2:
paths = [path[time:time + 2], (pos, successor)]
else:
paths = [[path[-1]], (pos, successor)]
if util.paths_conflict(paths):
break
else:
filtered.append(successor)
return filtered
def _reverse_path(self, state, came_from):
path = [state[1]]
while state in came_from:
state = came_from[state]
path.append(state[1])
path.reverse()
return path
def __repr__(self):
return f'{self.start}-{self.goal}'