def simple_poc(agents):
# Calculate the initial optimal set of paths
paths = []
for agent in agents:
agent.plan()
paths.append(agent.path)
conflicts = util.paths_conflict(paths)
while conflicts:
#print(f'Conflicts found: {len(conflicts)}')
#pprint(conflicts)
conflict_sets = find_conflicts(agents, conflicts)
# Randomly get an order for the conflict
for conflict in conflict_sets:
conflict_sets[conflict] = list(conflict_sets[conflict])
for agent in range(len(conflict_sets[conflict])):
conflict_sets[conflict][agent].stable_prio.extend(
conflict_sets[conflict][:agent])
# Replan for all agents
for agent in agents:
agent.plan()
paths = [agent.path for agent in agents]
conflicts = util.paths_conflict(paths)
# Get final paths
paths = list(agent.path for agent in agents)
conflicts = util.paths_conflict(paths)
#print(f'Final conflicts found: {len(conflicts)}')
return paths
def poc(agents, start_time=None, max_time=1):
paths = []
for agent in agents:
agent.plan(start_time=start_time, max_time=max_time)
paths.append(agent.path)
count = 0
conflicts = util.paths_conflict(paths)
init_conflicts = len(find_conflicts(agents, conflicts))
while conflicts:
time = timeit.default_timer()
if start_time != None and (time - start_time) > max_time:
raise TimeExceeded()
#print('Exporting conflicts')
count += 1
#print(f'Conflicts found: {len(conflicts)}')
#pprint(conflicts)
# Add the conflicts to the agents
conflict_sets = find_conflicts(agents, conflicts)
#pprint(conflict_sets)
for conflict in conflict_sets:
c = Conflict(conflict[0], conflict[1], conflict_sets[conflict])
for agent in c.agents:
agent.conflicts.add(c)
# Get the agents to resolve a conflict
for agent in agents:
try:
conflict = agent.conflicts[0]
except IndexError:
continue
# Clear agents from each others priorities before resolving
for agent1 in conflict.agents:
agent1.stable_prio = list(set(agent1.stable_prio))
for agent2 in conflict.agents:
try:
agent1.stable_prio.remove(agent2)
except ValueError:
pass # Ignore if agent is not in the list
conflict.resolve(start_time=start_time, max_time=max_time)
for agent in conflict.agents:
agent.conflicts.remove(conflict)
agent.old_conflicts.add(conflict)
# Calculate the final paths
paths = [agent.path for agent in agents]
conflicts = util.paths_conflict(paths)
#print(f'Final conflicts found: {len(conflicts)}')
# Output final conflict free paths
# Find number of conflicts solved
conflicts = set()
for agent in agents:
conflicts.update(agent.old_conflicts)
return {
'paths': paths,
'initial': init_conflicts,
'solved': len(conflicts),
}
def dimpp(agents, start_time, max_time):
n = len(agents)
for a in range(n):
agents[a].plan([], start_time, max_time)
global_plan = [agents[a].path]
try:
for i in range(1, n):
cur_time = timeit.default_timer()
if start_time != None and (cur_time - start_time) > max_time:
raise TimeExceeded()
j = (a + i) % n
agents[j].plan([], start_time, max_time)
# Go to next agent if there are no conflicts
conflicts = util.paths_conflict(global_plan + [agents[j].path])
if len(conflicts) == 0:
global_plan.append(agents[j].path)
continue
# Try to insert a wait in the plan at the first conflict
time = min(conflicts, key=lambda c: c['time'])['time']
if time < len(agents[j].path):
agents[j].path.insert(time, agents[j].path[time])
# Check if solved
conflicts = util.paths_conflict(global_plan + [agents[j].path])
if len(conflicts) == 0:
global_plan.append(agents[j].path)
continue
# If still not solved replan with constraints
agents[j].plan(global_plan, start_time, max_time)
conflicts = util.paths_conflict(global_plan + [agents[j].path])
# If there are still conflicts than finding a solution
# didn't work
if len(conflicts) > 0:
raise util.NoPathsFoundException()
global_plan.append(agents[j].path)
except util.NoPathsFoundException:
continue
# If this is reached then we should have a good solution
conflicts = util.paths_conflict(global_plan)
assert len(conflicts) == 0
global_plan = global_plan[n - a:] + global_plan[:n - a]
assert len(global_plan) == n
assert global_plan[0][0] == agents[0].start
return {'paths': global_plan, 'initial': 0, 'solved': 0}
# If we go through all agents and no plan has been found then we've failed
raise util.NoPathsFoundException
def groups_conflict(groups):
num_groups = len(groups)
conflicts = []
for group1 in range(num_groups):
for group2 in range(group1 + 1, num_groups):
for path1 in groups[group1].paths:
for path2 in groups[group2].paths:
if util.paths_conflict([path1, path2]):
conflicts.append((group1, group2))
return conflicts
def test_paths_conflict(self):
paths = (((0,0), (1,1), (2,2), (3,3), (4,4)),
((2,2), (1,1), (1,2), (1,3), (2,4)),
((2,5), (1,4), (1,3), (1,2), (0,2)),
((1,2), (2,3), (3,3), (3,4), (4,3)))
conflicts = util.paths_conflict(paths)
self.assertCountEqual(conflicts, [
{'path1': 0, 'path2': 1, 'time': 0},
{'path1': 1, 'path2': 2, 'time': 2},
{'path1': 0, 'path2': 3, 'time': 3},
])
def resolve(self, agents, start_time, max_time):
# Don't try to solve a conflict after having already done so
if self.solution:
return
# If this is not the first conflict for an agent then don't bother
for agent in self.agents:
if agent.conflicts[0] != self:
return
agent.current_conflict = self
#print(f'Resolving conflict {self}')
# Let the agents propose priorities
proposals = tuple(agent.propose(self) for agent in self.agents)
# Enter voting if there are multiple proposals
if len(proposals) > 1:
for proposal in proposals:
#print('Evaluation proposal', proposal)
self.proposals.append(proposal)
self.proposal = proposal
proposal['score'] = 0
# Plan new paths
for i in range(proposal['level'] + 1):
proposal[0].plan(start_time, max_time)
# Replanning for all agents shouldnt be too bad because
# of caching
for agent in self.agents:
agent.plan(start_time, max_time)
# Evaluate new paths
try:
for agent in self.agents:
paths = tuple(a.path for a in agents)
conflicts = util.paths_conflict(paths)
conflicts = convert_conflicts(agents, conflicts)
proposal['score'] += agent.evaluate(conflicts)
except ConflictNotSolved:
proposal['score'] = -float('inf')
continue
# Pick the proposal with the highest sum of votes
#pprint(self.proposals)
self.solution = max(self.proposals, key=lambda p: p['score'])
else:
self.solution = proposals[0]
self.proposal = None
#print('SOLUTION', self.solution)
# Tell the agents that we are done
for agent in self.agents:
agent.current_conflict = None
agent.resolved_conflict(self)
agent.plan(start_time, max_time)
def successor_states(world, current, goals, start_time, max_time):
for succ in rec_successor_states(world, current, 0):
time = timeit.default_timer()
if start_time != None and (time - start_time) > max_time:
raise TimeExceeded()
if util.paths_conflict(tuple(zip(current, succ))):
print('Conflicting paths found', current, succ)
continue
score = sum(1 for i in range(len(current)) if goals[i] != succ[i])
yield score, tuple(succ)
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 _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 test_paths_conflict_when_going_through_end_point(self):
paths = (((0,0), (1, 1), (2,2), (3,3)),
((2,1), (2,2)))
conflicts = util.paths_conflict(paths)
self.assertEqual(conflicts, [{'path1': 0, 'path2': 1, 'time': 2}])
def test_paths_conflict_cross_east_to_west(self):
paths = (((1,1), (0,0)), ((1,0), (0,1)))
conflicts = util.paths_conflict(paths)
self.assertEqual(conflicts, [{'path1': 0, 'path2': 1, 'time': 0}])
def test_paths_conflict_same_position(self):
paths = (((0,0), (1,1)), ((2,2), (1,1)))
conflicts = util.paths_conflict(paths)
self.assertEqual(conflicts, [{'path1': 0, 'path2': 1, 'time': 0}])
def test_paths_conflict_swap(self):
paths = (((0,0), (1,1)), ((1,1), (0,0)))
conflicts = util.paths_conflict(paths)
self.assertEqual(conflicts, [{'path1': 0, 'path2': 1, 'time': 0}])
def version1(agents, start_time, max_time, visualize=False):
paths = []
for agent in agents:
agent.plan(start_time=start_time, max_time=max_time)
paths.append(agent.path)
if visualize:
vis = visualisation.Visualisation(agents[0].world,
len(agents),
scale=20)
count = 0
conflicts = util.paths_conflict(paths)
init_conflicts = len(convert_conflicts(agents, conflicts))
while conflicts:
time = timeit.default_timer()
if start_time != None and (time - start_time) > max_time:
raise TimeExceeded()
if visualize:
print('Exporting conflicts')
im = vis.draw_paths_with_conflicts(paths, conflicts)
im.save(f'conflict_{count:05}.png')
count += 1
#print(f'Conflicts found: {len(conflicts)}')
#pprint(conflicts)
conflict_objs = convert_conflicts(agents, conflicts)
#pprint(conflict_objs)
# Add conflicts to agents
for agent in agents:
agent.conflicts.clear()
for conflict in conflict_objs.values():
for agent in conflict.agents:
agent.conflicts.add(conflict)
# Get the agents to resove the conflicts
for agent in agents:
try:
conflict = agent.conflicts[0]
except IndexError:
continue # Agent has no conflicts
conflict.resolve(agents, start_time, max_time)
# Update the list of conflicts
paths = [agent.path for agent in agents]
conflicts = util.paths_conflict(paths)
#print() # Just a new line to break up iterations
# Final visualisation
if visualize:
print('Exporting final conflicts')
im = vis.draw_paths_with_conflicts(paths, conflicts)
im.save(f'conflict_{count:05}.png')
# Find number of conflicts solved
conflicts = set()
for agent in agents:
conflicts.update(agent.resolved_conflicts)
# Find the size of conflicts
sizes = (len(c.agents) for c in conflicts)
return {'paths': paths,
'initial': init_conflicts,
'solved': len(conflicts),
'sizes': sizes,
}
def resolve(self, agents, start_time, max_time):
# Don't try to solve a conflict after having already done so
if self.solution:
return
# If this is not the first conflict for an agent then don't bother
for agent in self.agents:
if agent.conflicts[0] != self:
return
agent.current_conflict = self
#print(f'Resolving conflict {self}')
best_score = -float('inf')
partially_solved = False
while best_score == -float('inf') or partially_solved:
time = timeit.default_timer()
if start_time != None and (time - start_time) > max_time:
raise TimeExceeded()
partially_solved = True
proposals = tuple(filter(lambda p: p != None,
(agent.propose(self) for agent in self.agents)))
self.proposals += proposals
# If no proposals have been made then go with the best one so far
if len(proposals) == 0:
self.solution = max(self.proposals, key=lambda p: p['score'])
if self.solution['score'] == -float('inf'):
raise ConflictNotSolved()
break
if len(proposals) == 1:
self.solution = proposals[0]
break
# Evaluate the proposals
for proposal in proposals:
#print('Evaluation proposal', proposal)
self.proposal = proposal
proposal['score'] = 0
# Plan new paths
for i in range(proposal['level']):
proposal[0].plan(start_time, max_time)
# Replanning for all agents shouldnt be too bad because
# of caching
for agent in self.agents:
agent.plan(start_time, max_time)
# Evaluate new paths
try:
for agent in self.agents:
paths = tuple(a.path for a in agents)
conflicts = util.paths_conflict(paths)
conflicts = convert_conflicts(agents, conflicts)
proposal['score'] += agent.evaluate(conflicts)
except ConflictNotSolved:
proposal['score'] = -float('inf')
continue
except ConflictPartiallySolved as e:
proposal['score'] += e.args[0]
else:
partially_solved = False
#print(f"Proposal score {proposal['score']}")
# Pick the proposal with the highest sum of votes
#pprint(self.proposals)
self.solution = max(self.proposals, key=lambda p: p['score'])
best_score = max(p['score'] for p in self.proposals)
#print(f'Best score: {best_score}')
self.leaf_proposals = sorted((p for p in self.proposals
if not p['children']),
key=lambda p: p['score'])
self.proposal = None
#print('SOLUTION', self.solution)
# Tell the agents that we are done
for agent in self.agents:
agent.current_conflict = None
agent.resolved_conflict(self)
agent.plan(start_time, max_time)