def test_file():
fname = "/tmp/test.pkl"
config = generate_config()
config['filename_pathsave'] = fname
if os.path.exists(fname):
os.remove(fname)
assert not os.path.exists(fname), "File exists already"
agent_idle, agent_job, agent_pos, grid, idle_goals, jobs = get_data_labyrinthian(
3)
start_time = datetime.datetime.now()
plan_cbsext(agent_pos, jobs, [], idle_goals, grid, config)
time1 = (datetime.datetime.now() - start_time).total_seconds()
assert os.path.isfile(fname), "Algorithm has not created a file"
start_time = datetime.datetime.now()
plan_cbsext(agent_pos, jobs, [], idle_goals, grid, config)
time2 = (datetime.datetime.now() - start_time).total_seconds()
try:
assert time2 < time1, "It was not faster to work with saved data"
print("Saving path_save saved us", 100 * (time1 - time2) / time1,
"% of time")
finally:
os.remove(fname)
assert not os.path.exists(fname), "File exists after delete"
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 planner_comparison(seed):
params = get_data_random(seed + 1,
map_res=8,
map_fill_perc=30,
agent_n=5,
job_n=5,
idle_goals_n=0)
agent_pos, grid, idle_goals, jobs = params
mapstr = get_map_str(grid)
print(mapstr)
maphash = str(hashlib.md5(mapstr.encode('utf-8')).hexdigest())[:8]
print(maphash)
fname = "planner/eval/cache/" + str(
maphash) + '.pkl' # unique filename based on map
pre_calc_paths(jobs, idle_goals, grid, fname)
config_opt = generate_config()
config_opt['params'] = params
config_opt['filename_pathsave'] = fname
config_milp = config_opt.copy()
config_milp['milp'] = 1
config_cobra = config_opt.copy()
config_cobra['cobra'] = 1
config_greedy = config_opt.copy()
config_greedy['greedy'] = 1
config_nn = config_opt.copy()
config_nn['number_nearest'] = 2
config_col = config_nn.copy()
config_col['all_collisions'] = True
if is_cch():
print("Configs: [config_opt, config_nn, config_milp]")
configs = [config_opt, config_nn,
config_milp] #, config_cobra, config_greedy, config_col]
sizes = [1, 2]
else:
print(
"Configs: [config_opt, config_nn, config_milp, config_cobra, config_greedy]"
)
configs = [
config_opt, config_nn, config_milp, config_cobra, config_greedy
]
sizes = [2, 3, 4]
ts, ress = benchmark(one_planner, [configs, sizes],
samples=1,
timeout=1000)
return ts, ress
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_vertexswap():
from planner.planner_demo_vertexswap import values_vertexswap
grid, agent_pos, jobs, _, alloc_jobs, start_time = values_vertexswap()
config = generate_config()
config['filename_pathsave'] = ''
(_, _, res_paths) = plan_cbsext(agent_pos,
jobs,
alloc_jobs, [],
grid,
plot=False,
config=config)
assert not has_vortex_collision(
res_paths), "There are collisions in vortexes!"
assert not has_edge_collision(res_paths), "There are collisions in edges!"
def test_basic():
agent_idle, agent_job, agent_pos, grid, idle_goals, jobs = get_data_labyrinthian(
)
start_time = datetime.datetime.now()
config = generate_config()
config['filename_pathsave'] = ''
res_agent_job, res_agent_idle, res_paths = plan_cbsext(
agent_pos, jobs, [], idle_goals, grid, config)
print("computation time:",
(datetime.datetime.now() - start_time).total_seconds(), "s")
assert res_agent_job == agent_job, "wrong agent -> job assignment"
assert res_agent_idle == agent_idle, "wrong agent -> idle_goal assignment"
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) // chandra commented, no function defined in header file
# print(mapstr) // chandra commented
print("PLAN")
print("-----------------------GREEDY-----------------------")
greedy_time = time.time()
minlp_res_agent_job, minlp_res_paths = plan_greedy(agent_pos, jobs, grid,
config)
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)
print("--- Time taken is %s seconds ---" % (time.time() - greedy_time))
#
#plan_1(agent_pos, jobs, grid, config)
# # MINLP VS PLAN
# if display:
# fig = plt.figure()
# ax1 = fig.add_subplot(121, projection='3d')
# ax2 = fig.add_subplot(122, projection='3d')
# plot_results(ax1, [], res_paths, res_agent_job, agent_pos, grid, [], jobs)
# plot_results(ax2, [], minlp_res_paths, minlp_res_agent_job, agent_pos, grid, [], jobs)
# plt.show()
# print("-----------------------TCBS-----------------------")
# tcbs_time = time.time()
# 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_tcbs = get_costs(res_paths, jobs, res_agent_job, display)
# print("--- Time taken is %s seconds ---" % (time.time() - tcbs_time))
return 0 #costs_tcbs, #costs_minlp
def test_collision():
grid = np.zeros([10, 10, 50])
grid[2:8, 0:4, :] = -1
grid[2:8, 5:10, :] = -1
agent_pos = [(3, 1), (5, 1)]
idle_goals = [((3, 9), (8, .1)), ((5, 9), (8, .1))]
config = generate_config()
config['filename_pathsave'] = ''
res_agent_job, res_agent_idle, res_paths = plan_cbsext(agent_pos, [], [],
idle_goals,
grid,
config,
plot=False)
assert np.array(map(lambda x: len(x) == 0,
res_agent_job)).all(), "We don't have to assign jobs"
assert not has_vortex_collision(
res_paths), "There are collisions in vortexes!"
assert not has_edge_collision(res_paths), "There are collisions in edges!"
def plan_process(pipe, agent_pos, jobs, alloc_jobs, idle_goals, grid, fname):
config = generate_config()
config['filename_pathsave'] = fname
try:
(agent_job, agent_idle, paths) = plan(agent_pos, jobs, alloc_jobs,
idle_goals, grid, config)
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 test_idle_goals(plot=False):
grid = np.zeros([10, 10, 50])
agent_pos = [(3, 8), (0, 1)]
idle_goals = [
((3, 9), (2, 4)),
]
jobs = [
((0, 0), (9, 9), 1),
]
config = generate_config()
config['filename_pathsave'] = ''
res_agent_job, res_agent_idle, res_paths = plan_cbsext(
agent_pos=agent_pos,
jobs=jobs,
alloc_jobs=[],
idle_goals=idle_goals,
grid=grid,
config=config,
plot=plot)
assert res_agent_job == ((), (0, ))
assert res_agent_idle == ((0, ), ())
def test_same_jobs(plot=False):
grid = np.zeros([10, 10, 50])
agent_pos = [(4, 3), (4, 4)]
idle_goals = [((3, 9), (8, .1)), ((5, 9), (8, .1))]
jobs = [((0, 0), (9, 9), 0.358605), ((0, 0), (9, 9), 0.002422)]
config = generate_config()
config['filename_pathsave'] = ''
res_agent_job, res_agent_idle, res_paths = plan_cbsext(
agent_pos=agent_pos,
jobs=jobs,
alloc_jobs=[],
idle_goals=idle_goals,
grid=grid,
config=config,
plot=plot)
print(res_agent_idle)
# assert len(res_agent_idle[0]) == 0, "We don't have to assign idle goals"
# assert len(res_agent_idle[1]) == 0, "We don't have to assign idle goals"
# assert len(res_agent_job) == 2, "Not both jobs assigned"
# assert len(res_agent_job[0]) == 1, "Not all jobs assigned to one agent"
# assert len(res_agent_job[1]) == 1, "Not all jobs assigned to one agent"
assert len(res_paths) == 2, "Not two path sets for the agents"
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()
def test_consecutive_jobs():
grid = np.zeros([10, 10, 50])
agent_pos = [(1, 1)]
idle_goals = [((3, 9), (8, .1)), ((5, 9), (8, .1))]
jobs = [((2, 0), (2, 9), -6), ((7, 3), (3, 3), -1.5), ((3, 4), (5, 1), 0)]
config = generate_config()
config['filename_pathsave'] = ''
res_agent_job, res_agent_idle, res_paths = plan_cbsext(
agent_pos=agent_pos,
jobs=jobs,
alloc_jobs=[],
idle_goals=idle_goals,
grid=grid,
config=config,
plot=False)
assert len(res_agent_idle[0]) == 0, "We don't have to assign idle goals"
assert len(res_agent_job) == 1, "Not one assigned job"
assert len(res_agent_job[0]) == 3, "Not all jobs assigned to first agent"
assert len(res_paths) == 1, "Not one path sets for the agent"
assert len(
res_paths[0]
) == 6, "Not six paths for the agent" # being six due to the oaths to start
def test_rand():
for i in range(5):
print("\nTEST", i)
r = random.SystemRandom()
seed = r.randint(0, 1024)
agent_pos, grid, idle_goals, jobs = get_data_random(
seed, 10, 5, 3, i, 5)
start_time = datetime.datetime.now()
config = generate_config()
config['filename_pathsave'] = ''
res_agent_job, res_agent_idle, res_paths = plan_cbsext(
agent_pos, jobs, [], idle_goals, grid, config)
print("computation time:",
(datetime.datetime.now() - start_time).total_seconds(), "s")
print("RESULTS:\nres_agent_job", res_agent_job)
print("res_agent_idle", res_agent_idle)
if res_paths is False:
logging.warning("NO SOLUTION")
else:
print("res_paths", res_paths)
import numpy as np
import matplotlib.pyplot as plt
from planner.tcbs.plan import generate_config, plan
from planner.eval.display import animate_results, plot_inputs
from planner.milp.milp import plan_milp
from tools import load_map
config = generate_config()
jobs = [
((0, 0), (2, 6), 0),
((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, [], [],
def handle_multi_planner(req):
#global map
_map = load_map(
os.path.realpath(os.path.join(__file__, "../../../")) +
'/real_robot_controller/map/' + sys.argv[1])
# print ('Map is: ' + _map)
# _map = rospy.wait_for_message("/map",OccupancyGrid) # change the topic based on the namespace
# map_resolution = round(_map.info.resolution,2)
# print(map_resolution)
map_resolution = 1
# map_width = _map.info.width
# map_height = _map.info.height
# map = np.array(_map.data)
# map = np.reshape(map,(map_height,map_width))
# print("map")
# print(len(map))
agent_pos = []
for pos in req.start:
agent_pos.append(
(round(pos.pose.position.x / map_resolution,
0), round(pos.pose.position.y / map_resolution,
0))) #round based on map resolution
it_jobs = [round(elem, 2) for elem in req.jobs]
print(it_jobs)
list_jobs = [it_jobs[i:i + 5] for i in range(0, len(it_jobs), 5)]
print(list_jobs)
jobs = []
for sublist in list_jobs:
tuple_start = (round(sublist[0] / map_resolution,
0), round(sublist[1] / map_resolution, 0))
tuple_goal = (round(sublist[2] / map_resolution,
0), round(sublist[3] / map_resolution, 0))
job = (tuple_start, tuple_goal, sublist[4])
print(job)
jobs.append(job)
grid = np.repeat(_map[:, ::2, np.newaxis], 100, axis=2)
# grid = np.repeat(_map[:, ::, np.newaxis], 100, axis=2)
config = generate_config()
config['filename_pathsave'] = req.fname
config['finished_agents_block'] = True
print("Problem:")
print(str(jobs))
print(str(agent_pos))
print("GREEDY")
gui_path_array = Path_array_agentjob()
res_agent_job, tuple_paths = plan_greedy(agent_pos, jobs, grid, config)
# res_agent_job, idle, tuple_paths = plan_tcbs(agent_pos, jobs, [], [], grid, config, plot=False)
max_job_size = max([len(i) for i in res_agent_job]) * 2
gui_path_array.max_job_size = max_job_size
for robo in res_agent_job:
agent_robo_job = agent_job()
for jobs in robo:
agent_robo_job.agent_robo_job.append(jobs)
gui_path_array.agent_job.append(agent_robo_job)
print("tuple_paths")
print(tuple_paths)
for agent in tuple_paths:
_seq = 0
_agent_paths = agent_paths()
for job in agent:
gui_path = Path()
Poses = []
for pos in job:
pose = PoseStamped()
pose.header.seq = _seq
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = pos[0] * map_resolution
pose.pose.position.y = pos[1] * map_resolution
pose.pose.position.z = 0.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 1.0
Poses.append(pose)
_seq = _seq + 1
gui_path.header.frame_id = "map"
gui_path.header.stamp = rospy.Time.now()
gui_path.poses = Poses
if gui_path.poses[0].pose != gui_path.poses[-1].pose:
_agent_paths.agent_paths.append(gui_path)
gui_path_array.path_array.append(_agent_paths)
return gui_path_array
