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 ff():
jobs = random_jobs(3, [(0, 0), (2, 0), (2, 6), (4, 6), (6, 2), (7, 7)])
_map = load_map('ff.png')
agent_pos = [(0, 1), (2, 7), (7, 4)]
fig = plt.figure()
ax = fig.add_subplot(111)
grid = np.repeat(_map[:, ::2, np.newaxis], 100, axis=2)
plot_inputs(ax, agent_pos, [], jobs, grid)
plt.show()
return eval(_map,
agent_pos,
jobs,
'ff.pkl',
finished_blocking=False,
display=True)
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()
((6, 2), (2, 0), 0),
((4, 6), (6, 2), 0),
]
grid = load_map('ff.png')
agent_pos = [(0, 0), (2, 6), (7, 7)]
config['filename_pathsave'] = 'ff.pkl'
grid = np.repeat(grid[:, ::2, np.newaxis], 100, axis=2)
params = {'legend.fontsize': 'small'}
plt.rcParams.update(params)
fc = plt.figure()
ax1 = fc.add_subplot(131)
fc.set_size_inches(9, 3.5)
plot_inputs(ax1, agent_pos, [], jobs, grid)
fc.savefig('scenario_ff_config.png')
tcbs_agent_job, tcbs_agent_idle, tcbs_paths = plan(agent_pos,
jobs, [], [],
grid,
config,
plot=False)
minlp_agent_job, minlp_paths = plan_milp(agent_pos, jobs, grid, config)
print((tcbs_paths, tcbs_agent_job, minlp_paths, minlp_agent_job, agent_pos,
jobs))
f = plt.figure()
f.set_size_inches(8, 4.5)
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)
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='')
f.savefig('random.png')
f.set_size_inches(19.20, 10.80)
plt.title("Problem Configuration")
plt.tight_layout()
f.savefig('video/random_problem.png')
if a not in agent_pos:
agent_pos.append(a)
start_time = datetime.datetime.now()
try:
res_agent_job, res_agent_idle, res_paths = plan(
agent_pos,
jobs, [],
idle_goals,
grid,
plot=True,
filename='map_test.pkl')
except RuntimeError:
# logging.warning("NO SOLUTION (exception)")
plot_inputs(agent_pos,
idle_goals,
jobs,
grid,
show=True,
subplot=111)
duration.append((datetime.datetime.now() - start_time).total_seconds())
results_mean[n_j_s.index(n_j), n_a_s.index(n_a)] = np.mean(duration)
results_std[n_j_s.index(n_j), n_a_s.index(n_a)] = np.std(duration)
print("\nmean ..")
print(results_mean)
print("\nstd ..")
print(results_std)
