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