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"