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 one_planner(config, size):
print("size=" + str(size))
print("Testing with number_nearest=" + str(config['number_nearest']))
print("Testing with all_collisions=" + str(config['all_collisions']))
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, plot=False
)
print(res_agent_job)
return get_costs(res_paths, jobs, res_agent_job, True)
def plan_process(pipe, agent_pos, jobs, alloc_jobs, idle_goals, grid, fname):
try:
(agent_job,
agent_idle,
paths) = plan(agent_pos,
jobs,
alloc_jobs,
idle_goals,
grid,
False,
fname)
except Exception as e:
# Could not find a solution, returning just anything .. TODO: something better?
logging.warning("Could not find a solution, returning just anything \n", str(e))
agent_job = []
for a in agent_pos:
agent_job.append(tuple(a))
agent_job[0] = (0,)
agent_idle = ()
paths = get_paths(comp2condition(agent_pos, jobs, alloc_jobs, idle_goals, grid),
comp2state(tuple(agent_job), agent_idle, ()))
pipe.send((agent_job,
agent_idle,
paths))
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
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)
ani = animate_results(f, [], tcbs_paths, tcbs_agent_job, agent_pos, grid, [],
jobs, 'TCBS')
ani.save("scenario_ff_tcbs.mp4", writer="ffmpeg", fps=10)
f = plt.figure()
print("test", i)
agent_pos, grid, idle_goals, jobs = get_data_random(
map_res=map_res,
map_fill_perc=5,
agent_n=agent_n,
job_n=agent_n,
idle_goals_n=agent_n)
try:
fname = ''
fname = pre_calc_paths(jobs, idle_goals, grid)
start_time = datetime.datetime.now()
res_agent_job, res_agent_idle, res_paths = plan(agent_pos,
jobs, [],
idle_goals,
grid,
filename=fname)
d = (datetime.datetime.now() - start_time).total_seconds()
# print("computation time:", d, "s")
duration.append(d)
# print("RESULTS:\nres_agent_job", res_agent_job)
# print("res_agent_idle", res_agent_idle)
if res_paths is False:
logging.warning("NO SOLUTION")
# planner.plan.plot_inputs(agent_pos, idle_goals, jobs, grid, show=True)
else:
pass
# print("res_paths", res_paths)
idle_goals = []
for i_l in range(len(landmarks)):
idle_goals.append((landmarks[i_l], (random.randint(0, 20),
random.randint(1, 50) / 10)))
agent_pos = []
while len(agent_pos) < n_a:
a = random.choice(agents)
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)