def one_planner(config, size):
print("size=" + str(size))
agent_pos, grid, idle_goals, jobs = config['params']
agent_pos = agent_pos[0:size]
jobs = jobs[0:size]
if 'milp' in config:
print("milp")
from planner.milp.milp import plan_milp
res_agent_job, res_paths = plan_milp(agent_pos, jobs, grid, config)
elif 'cobra' in config:
print("cobra")
from planner.cobra.funwithsnakes import plan_cobra
res_agent_job, res_paths = plan_cobra(agent_pos, jobs, grid, config)
elif 'greedy' in config:
print("greedy")
from planner.greedy.greedy import plan_greedy
res_agent_job, res_paths = plan_greedy(agent_pos, jobs, grid, config)
else:
res_agent_job, res_agent_idle, res_paths = plan(
agent_pos, jobs, [], idle_goals, grid, config
)
print(res_agent_job)
if is_cch():
fig = plt.figure()
ax1 = fig.add_subplot(121)
plot_inputs(ax1, agent_pos, [], jobs, grid)
ax2 = fig.add_subplot(122, projection='3d')
plot_results(ax2, [], res_paths, res_agent_job, agent_pos, grid, [], jobs)
plt.show()
return get_costs(res_paths, jobs, res_agent_job, True)
def test_read_map(fname='planner/cobra/test.path', plot=False):
print("cwd: " + str(os.getcwd()))
grid = np.zeros([10, 10, 100])
paths = read_path_file(fname, grid)
if plot:
plot_results([], paths, [], [], plt.figure(), grid, [], [])
assert len(paths) == 3, "Not all agents have paths"
assert len(paths[0][0]) == 30, "No full paths"
def test_cobra_simple(plot=False):
grid = np.zeros([5, 5, 30])
res_agent_job, res_paths = plan_cobra([(1, 1), (2, 2)],
[((3, 3), (1, 4), 0),
((4, 1), (0, 0), 0)], grid,
generate_config())
if plot:
plot_results([], res_paths, [], [], plt.figure(), grid, [], [])
assert res_agent_job, "No result"
assert res_paths, "No result"
def eval(_map, agent_pos, jobs, fname, display=False, finished_blocking=True):
from planner.milp.milp import plan_milp
grid = np.repeat(_map[:, ::2, np.newaxis], 100, axis=2)
config = generate_config()
config['filename_pathsave'] = fname
config['finished_agents_block'] = finished_blocking
print("Problem:")
print(str(jobs))
print(str(agent_pos))
mapstr = get_map_str(grid)
print(mapstr)
print("TCBS")
res_agent_job, res_agent_idle, res_paths = plan(agent_pos,
jobs, [], [],
grid,
config,
plot=False)
print("agent_job: " + str(res_agent_job))
print("paths: " + str(res_paths))
costs_opt = get_costs(res_paths, jobs, res_agent_job, display)
print("MINLP")
minlp_res_agent_job, minlp_res_paths = plan_milp(agent_pos,
jobs,
grid,
config,
plot=False)
print("agent_job: " + str(minlp_res_agent_job))
print("paths: " + str(minlp_res_paths))
costs_minlp = get_costs(minlp_res_paths, jobs, minlp_res_agent_job,
display)
# MINLP VS PLAN
if display:
fig = plt.figure()
ax1 = fig.add_subplot(121, projection='3d')
ax1.set_title("Solution Optimal")
ax2 = fig.add_subplot(122, projection='3d')
plot_results(ax1, [], res_paths, res_agent_job, agent_pos, grid, [],
jobs, "Optimal")
plot_results(ax2, [], minlp_res_paths, minlp_res_agent_job, agent_pos,
grid, [], jobs, "Separate")
plt.show()
print("TCBS (results again for comparison)")
print("agent_job: " + str(res_agent_job))
print("paths: " + str(res_paths))
costs_tcbs = get_costs(res_paths, jobs, res_agent_job, display)
return costs_tcbs, costs_minlp
def test_cobra_random(plot=False):
agent_pos, grid, idle_goals, jobs = get_data_random(seed=1,
map_res=8,
map_fill_perc=20,
agent_n=3,
job_n=3,
idle_goals_n=0)
res_agent_job, res_paths = plan_cobra(agent_pos, jobs, grid,
generate_config())
print(res_agent_job)
all_alloc = reduce(lambda a, b: a + b, res_agent_job, tuple())
jobs_is = list(range(len(jobs)))
for i_j in all_alloc:
jobs_is.remove(i_j)
assert not jobs_is, "Not all jobs allocated"
if plot:
fig = plt.figure()
ax = fig.add_subplot(121)
plot_inputs(ax, agent_pos, idle_goals, jobs, grid)
ax2 = fig.add_subplot(122, projection='3d')
plot_results(ax2, [], res_paths, res_agent_job, agent_pos, grid, [],
jobs)
plt.show()
from planner.eval.display import plot_results
grid = np.zeros([10, 10, 51])
grid[4, 1:9, :] = -1
def pathplan(agent_pos, jobs):
start_time = datetime.datetime.now()
# This misuses the cbsext planner as cbs only planner by fixing the assignment
res_agent_job, res_agent_idle, res_paths = plan_cbsext(
agent_pos,
jobs, [], [],
grid,
plot=False,
filename='pathplanning_only.pkl',
pathplanning_only_assignment=[(0, ), (1, ), (2, )])
print("computation time:",
(datetime.datetime.now() - start_time).total_seconds(), "s")
return res_paths
# input 1
agent_pos = [(1, 1), (2, 1), (3, 1)] # three agents
jobs = [((1, 7), (9, 1), 0), ((1, 8), (8, 1), 0),
((9, 8), (4, 2), 0)] # three jobs
paths = pathplan(agent_pos, jobs)
for p in paths:
print(p)
plot_results([], paths, [], agent_pos, plt.figure(), grid, [], jobs)
def plan(agent_pos, jobs, alloc_jobs, idle_goals, grid,
config = {}, plot = False, pathplanning_only_assignment=False):
"""
Main entry point for planner
Args:
agent_pos: agent poses
jobs: jobs to plan for (((s_x, s_y), (g_x, g_y), time), ((s_x ...), ..., time), ...)
where time might be negative value if job is waiting
alloc_jobs: preallocation of agent to a job (i.e. will travel to its goal)
idle_goals: idle goals to consider (((g_x, g_y), (t_mu, t_std)), ...)
grid: the map (2D-space + time)
plot: whether to plot conditions and results or not
filename: filename to save / read path_save (set to False to not do this)
agent_pos: list:
jobs: list:
alloc_jobs: list:
idle_goals: list:
grid: np.array:
plot: bool: (Default value = False)
filename: str: (Default value = 'path_save.pkl')
pathplanning_only_assignment: bool: do the pathplanning only (this assumes each job to the same index agent)
Returns:
: tuple of tuples of agent -> job allocations, agent -> idle goal allocations and blocked map areas
"""
if not config:
config = generate_config() # default config
filename = config['filename_pathsave']
global _config
_config = config
# load path_save
if filename: # TODO: check if file was created on same map
load_paths(filename)
pool = alloc_threads()
alloc_threads()
agent_job = []
_agent_idle = []
if _config['number_nearest'] != 0:
global _distances
filename = pre_calc_paths(jobs, idle_goals, grid, filename)
_distances = pre_calc_distances(agent_pos, jobs, idle_goals, grid, filename)
agent_pos_test = set()
for a in agent_pos:
# init agent_job allocation
agent_job.append(tuple())
_agent_idle.append(tuple())
# check for duplicate agents
assert a not in agent_pos_test, "Duplicate agent poses"
agent_pos_test.add(a)
if pathplanning_only_assignment:
logging.info("Pathplanning only!")
agent_job = pathplanning_only_assignment
# for agent in range(len(agent_job)):
# if len(agent_job[agent]) == 0: # no assignment
# new_job = ((0, 0), agent_pos[agent], 0) # fake job
# jobs.append(
# new_job
# )
# alloc_jobs.append(
# (agent, jobs.index(new_job))
# )
for aj in alloc_jobs:
agent_job[aj[0]] = (aj[1],)
agent_job = tuple(agent_job)
_agent_idle = tuple(_agent_idle)
# making jobs unique
jobs = make_unique(jobs)
blocked = ()
condition = comp2condition(agent_pos, jobs, alloc_jobs, idle_goals, grid)
# planning!
(agent_job, _agent_idle, blocked
) = astar_base(start=comp2state(agent_job, _agent_idle, blocked),
condition=condition,
goal_test=goal_test,
get_children=get_children,
heuristic=heuristic,
cost=cost)
_paths = get_paths(condition, comp2state(agent_job, _agent_idle, blocked))
# save path_save
if filename:
save_paths(filename)
if plot:
fig = plt.figure()
ax1 = fig.add_subplot(121)
plot_inputs(ax1, agent_pos, idle_goals, jobs, grid)
ax2 = fig.add_subplot(122, projection='3d')
plot_results(ax2, _agent_idle, _paths, agent_job, agent_pos, grid, idle_goals, jobs)
plt.show()
pool.close()
pool.terminate()
pool.join()
return agent_job, _agent_idle, _paths
import numpy as np
from tools import load_map
from planner.greedy.greedy import plan_greedy
from planner.eval.display import plot_inputs, plot_results
_map = load_map('../map2.png')
_map = _map[:, ::2]
grid = np.repeat(_map[:, :, np.newaxis], 100, axis=2)
# input
agent_pos = [(0, 0), (1, 0), (2, 0)]
jobs = [((0, 8), (0, 2), 0),
((1, 8), (2, 4), 0),
((2, 8), (4, 8), 0),
((7, 6), (3, 8), 0),
((8, 7), (8, 2), 0),
((3, 4), (5, 8), 0)]
idle_goals = []
fig = plot_inputs(agent_pos, idle_goals, jobs, grid, show=False)
(res_agent_job, res_paths) = plan_greedy(agent_pos, jobs, grid, 'greedy.pkl')
plot_results([], res_paths, res_agent_job, agent_pos, fig, grid, idle_goals, jobs)
job_n=4,
idle_goals_n=0)
agent_pos, grid, idle_goals, jobs = params
mapstr = get_map_str(grid)
maphash = str(hashlib.md5(mapstr.encode('utf-8')).hexdigest())[:8]
fname = "planner/eval/cache/" + str(maphash) + '.pkl' # unique filename based on map
config = generate_config()
config['finished_agents_block'] = True
config['filename_pathsave'] = fname
tcbs_agent_job, tcbs_agent_idle, tcbs_paths = plan(agent_pos, jobs, [], [], grid, config, plot=False)
f = plt.figure()
ax0= f.add_subplot(111)
plot_results(ax0, tcbs_agent_idle, tcbs_paths, tcbs_agent_job, agent_pos, grid, [], jobs, "tcbs")
milp_agent_job, milp_agent_idle, milp_paths = plan_milp(agent_pos, jobs, [], [], grid, config, plot=False)
f1 = plt.figure()
ax1= f1.add_subplot(111)
plot_results(ax1, milp_agent_idle, milp_paths, milp_agent_job, agent_pos, grid, [], jobs, "milp")
print(str((tcbs_paths, tcbs_agent_job, milp_paths, milp_agent_job, agent_pos, jobs)))
params = {'legend.fontsize': 'small'}
plt.rcParams.update(params)
f = plt.figure()
ax1 = f.add_subplot(111)
f.set_size_inches(4, 4)
plot_inputs(ax1, agent_pos, [], jobs, grid, title='')