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 plan_milp(agent_pos, jobs, grid, config, plot=False):
res_agent_job = optimize(agent_pos, jobs)
_, _, res_paths = plan_cbsext(agent_pos, jobs, [], [], grid, config,
plot=plot,
pathplanning_only_assignment=res_agent_job)
return res_agent_job, res_paths
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
def plan_greedy(agent_pos, jobs, grid, config):
filename = config['filename_pathsave']
load_paths(filename)
res_agent_job = strictly_consec(agent_pos, jobs, grid)
assertjobs = reduce(lambda a, b: a + b, res_agent_job, tuple())
for ij in range(len(jobs)):
assert ij in assertjobs
save_paths(filename)
print(res_agent_job)
_, _, res_paths = plan_cbsext(agent_pos, jobs, [], [], grid,
plot=False,
config=config,
pathplanning_only_assignment=res_agent_job)
return res_agent_job, res_paths
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 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 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 plan_1(agent_pos, jobs, grid, config):
print ("Agents Positions ")
print ( agent_pos )
print ("Jobs ")
print ( jobs )
# print ("Grid ")
# print ( grid )
print ("config ")
print (config)
filename = config['filename_pathsave']
load_paths(filename)
res_agent_job = try_assign(agent_pos, jobs, grid)
assertjobs = reduce(lambda a, b: a + b, res_agent_job, tuple())
for ij in range(len(jobs)):
assert ij in assertjobs
save_paths(filename)
# print(res_agent_job)
_, _, res_paths = plan_cbsext(agent_pos, jobs, [], [], grid,
plot=False,
config=config,
pathplanning_only_assignment=res_agent_job)
return res_agent_job, res_paths
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)
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
logging.getLogger('pyutilib.component.core.pca').setLevel(logging.INFO)
_map = load_map('map2.png')
_map = _map[:, ::2]
grid = np.repeat(_map[:, :, np.newaxis], 100, axis=2)
# input
agent_pos = [(0, 0), (3, 0), (2, 1)]
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)
]
idle_goals = []
# print("MILP")
# start_time = datetime.datetime.now()
# res_agent_job, res_paths = plan_milp(agent_pos, jobs, grid, filename='map2_test.pkl')
# print("computation time:", (datetime.datetime.now() - start_time).total_seconds(), "s")
# print(res_agent_job, res_paths)
print("CBSEXT")
start_time = datetime.datetime.now()
res_agent_job, res_agent_idle, res_paths = plan_cbsext(
agent_pos, jobs, [], idle_goals, grid, plot=True, filename='map2_test.pkl')
print("computation time:",
(datetime.datetime.now() - start_time).total_seconds(), "s")
print(res_agent_job, res_agent_idle, res_paths)
grid[8, 4:9, :] = -1
# input
agent_pos = [(1, 1), (1, 5)]
jobs = [
((2, 0), (2, 9), 0),
((7, 3), (3, 3), 0),
((3, 5), (7, 1), 0),
((3, 6), (6, 1), 0),
# ((3, 7), (5, 1), 0),
((3, 8), (4, 1), 0)
]
idle_goals = [((9, 7), (5, .5)), ((9, 8), (20, .5))]
alloc_jobs = []
start_time = datetime.datetime.now()
(res_agent_job, res_agent_idle,
res_paths) = plan_cbsext(agent_pos,
jobs,
alloc_jobs,
idle_goals,
grid,
plot=True,
filename='too_many_jobs.pkl')
print("computation time:",
(datetime.datetime.now() - start_time).total_seconds(), "s")
print(res_agent_job, res_agent_idle, res_paths)
import datetime
import numpy as np
from planner.tcbs.plan import plan_cbsext
grid = np.zeros([20, 20, 101])
grid[4, 0:16, :] = -1
grid[4, 17:20, :] = -1
# input
agent_pos = [(8, 9), (9, 9)]
jobs = [((9, 9), (4, 0), -16), ((9, 9), (4, 0), -12)]
idle_goals = [((9, 7), (5, .5)), ((9, 8), (20, .5))]
alloc_jobs = []
start_time = datetime.datetime.now()
(res_agent_job, res_agent_idle, res_paths) = plan_cbsext(agent_pos,
jobs,
alloc_jobs,
idle_goals,
grid,
plot=True,
filename='')
print("computation time:",
(datetime.datetime.now() - start_time).total_seconds(), "s")
print(res_agent_job, res_agent_idle, res_paths)
n_j = 4
n_a = 4
jobs = []
while len(jobs) < n_j:
j = (random.choice(landmarks), random.choice(landmarks),
random.randint(0, 5))
jobs.append(j)
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()
config = generate_config()
config["filename_pathsave"] = 'planner/map_test.pkl'
res_agent_job, res_agent_idle, res_paths = plan_cbsext(agent_pos,
jobs, [],
idle_goals,
grid,
plot=True,
config=config)
res2 = np.zeros([len(n_agent), n_tests])
for i_agents in range(len(n_agent)):
print("i_agents", i_agents)
for i_test in range(n_tests):
print("i_test", i_test)
if os.path.exists(fname):
os.remove(fname)
assert not os.path.exists(fname), "File exists already"
agent_pos, grid, idle_goals, jobs = get_data_random(
10, 5, n_agent[i_agents], n_agent[i_agents], 5)
start_time = datetime.datetime.now()
try:
plan_cbsext(agent_pos, jobs, [], idle_goals, grid, filename=fname)
except RuntimeError:
print("NO SOLUTION")
time1 = (datetime.datetime.now() - start_time).total_seconds()
assert os.path.isfile(fname), "Algorithm has not created a file"
start_time = datetime.datetime.now()
try:
plan_cbsext(agent_pos, jobs, [], idle_goals, grid, filename=fname)
except RuntimeError:
print("NO SOLUTION")
time2 = (datetime.datetime.now() - start_time).total_seconds()
os.remove(fname)
assert not os.path.exists(fname), "File exists after delete"