def h(): _map = load_map('h.png') agent_pos = [(0, 0), (2, 2)] jobs = [ ((0, 1), (2, 0), 0), ((2, 1), (0, 2), 0), ] eval(_map, agent_pos, jobs, 'h.pkl')
def line(): _map = load_map('line.png') agent_pos = [(0, 0), (6, 0)] jobs = [ ((1, 0), (6, 0), 0), ((5, 0), (1, 0), 0), ] eval(_map, agent_pos, jobs, 'line.pkl')
def s(): _map = load_map('S.png') agent_pos = [(3, 4), (6, 0), (2, 0)] jobs = [ ((0, 4), (0, 0), 0), ((2, 4), (8, 0), 0), ((7, 0), (3, 4), 0), ] eval(_map, agent_pos, jobs, 'S.pkl')
def corridor(): _map = load_map('corridor.png') agent_pos = [(0, 0), (0, 1)] jobs = [ ((5, 0), (5, 2), 0), ((4, 2), (4, 0), 0), ((3, 0), (3, 2), 0), ] eval(_map, agent_pos, jobs, 'corridor.pkl')
def i(): _map = load_map('I.png') agent_pos = [(0, 2), (4, 1)] jobs = [ ((0, 0), (0, 3), 0), ((1, 3), (1, 0), 0), ((3, 0), (3, 3), 0), ((4, 3), (4, 0), 0), ] eval(_map, agent_pos, jobs, 'I.pkl')
def o(): _map = load_map('o.png') agent_pos = [ (1, 3), # (6, 1), # (2, 2), (1, 1) ] jobs = [((7, 4), (0, 4), 4), ((2, 0), (3, 7), 3), ((4, 5), (7, 5), 0), ((4, 4), (6, 5), 1)] eval(_map, agent_pos, jobs, 'o.pkl', finished_blocking=False, display=True)
def ff(): jobs = random_jobs(2, [(0, 0), (2, 0), (2, 6), (4, 6), (6, 2), (7, 7)]) _map = load_map('ff.png') agent_pos = [ (0, 0), # (2, 6), (7, 7) ] return eval(_map, agent_pos, jobs, 'ff.pkl', finished_blocking=False, display=False)
def u(): _map = load_map('u.png') agent_pos = [(2, 1), (3, 1), (4, 1)] jobs = [ ((1, 4), (3, 1), 0), ((2, 4), (4, 1), 0), ((3, 4), (5, 1), 0), ] # fig = plt.figure() # ax = fig.add_subplot(111) # grid = np.repeat(_map[:, ::2, np.newaxis], 100, axis=2) # plot_inputs(ax, agent_pos, [], jobs, grid) # fig.show() eval(_map, agent_pos, jobs, 'u.pkl', finished_blocking=False, display=True)
def iros18_workshop(): _map = load_map('ff.png') jobs = [((2, 0), (6, 2), 0), ((0, 0), (2, 6), 0), ((7, 7), (4, 6), 0)] agent_pos = [(0, 1), (2, 7), (7, 4)] # fig = plt.figure() # ax = fig.add_subplot(111) # grid = np.repeat(_map[:, ::2, np.newaxis], 100, axis=2) # plot_inputs(ax, agent_pos, [], jobs, grid) # plt.show() return eval(_map, agent_pos, jobs, 'ff.pkl', finished_blocking=False, display=True)
def ff(): jobs = random_jobs(3, [(0, 0), (2, 0), (2, 6), (4, 6), (6, 2), (7, 7)]) _map = load_map('ff.png') agent_pos = [(0, 1), (2, 7), (7, 4)] fig = plt.figure() ax = fig.add_subplot(111) grid = np.repeat(_map[:, ::2, np.newaxis], 100, axis=2) plot_inputs(ax, agent_pos, [], jobs, grid) plt.show() return eval(_map, agent_pos, jobs, 'ff.pkl', finished_blocking=False, display=True)
def mr_t(): _map = load_map('mr_t.png') agent_pos = [(5, 3), (2, 1)] jobs = [((4, 3), (4, 1), 0), ((3, 1), (3, 3), 0)] eval(_map, agent_pos, jobs, 'mr_t.pkl', finished_blocking=True)
(5, 1, 12), (5, 1, 13), (5, 1, 14), (5, 1, 15), (5, 1, 16), (5, 1, 17), (5, 1, 18), (5, 1, 19), (5, 1, 20)]), ([(3, 1, 0), (4, 1, 1)], [(4, 1, 2), (3, 1, 3), (3, 0, 4), (2, 0, 5), (1, 0, 6), (1, 1, 7), (1, 2, 8), (1, 3, 9), (1, 4, 10), (1, 5, 11), (1, 6, 12), (1, 7, 13), (2, 7, 14), (3, 7, 15), (3, 6, 16), (3, 5, 17), (3, 4, 18), (3, 3, 19), (4, 3, 20)]) ], [(0, ), (1, )], [(6, 3), (3, 1)], [((5, 3), (5, 1), 0), ((4, 1), (4, 3), 0)]) grid = load_map('mr_t.png') agent_pos = [(6, 3), (3, 1)] jobs = [((5, 3), (5, 1), 0), ((4, 1), (4, 3), 0)] grid = np.repeat(grid[:, ::2, np.newaxis], 100, axis=2) 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("video/scenario_tcbs.mp4", writer="ffmpeg", fps=10) f = plt.figure() f.set_size_inches(8, 4.5) ani = animate_results(f, [], minlp_paths, minlp_agent_job, agent_pos, grid, [], jobs, 'MINLP')
import datetime import logging import numpy as np from planner.tcbs.plan import plan as plan_cbsext # from planner.milp.milp import plan_milp from tools import load_map logging.getLogger('pyomo').setLevel(logging.INFO) 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")
Threading in Python : http://stackoverflow.com/a/11968818/5343977 """ from threading import Thread import time import numpy as np import math from matplotlib import pyplot as plt #from planner.common import path from tools import load_map import networkx as nx ###########MAP################### ''' Map: obstacles and size ''' map = load_map('o.png') #print (map) #grid = np.repeat(map[:, ::2, np.newaxis], 100, axis=2) #print ("Grid::: ", grid) #print grid.shape map = map[:, ::2] #print map n = map.shape[0] print map.shape G = nx.grid_graph([n, n]) #print ("G::: ", G.nodes()) obstacle = [] for n in G.nodes(): if not map[n] >= 0: # obstacle
from planner.tcbs.plan import plan, generate_config from planner.eval.eval_scenarios import get_costs from planner.milp.milp import plan_milp from tools import load_map from planner.eval.display import * from mpl_toolkits.mplot3d import Axes3D _ = Axes3D config = generate_config() config['finished_agents_block'] = True no_calc = True if True: grid = load_map('mr_t.png') agent_pos = [(6, 3), (3, 1)] jobs = [((5, 3), (5, 1), 0), ((4, 1), (4, 3), 0)] config['filename_pathsave'] = 'mr_t.pkl' else: jobs = [ ((0, 0), (2, 6), 0), ((6, 2), (2, 0), 0), ((4, 6), (6, 2), 0), ] # ((7, 7), (4, 6), 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)
import datetime import random import numpy as np from planner.tcbs.plan import plan_cbsext from tools import load_map _map = load_map('map.png') grid = np.repeat(_map[:, :, np.newaxis], 100, axis=2) landmarks = [(1, 1), (3, 1), (5, 1), (7, 1), (9, 1), (8, 5), (1, 6), (9, 7), (1, 8), (9, 9)] # input agents = [(1, 1), (3, 3), (5, 0), (7, 2), (9, 1), (5, 5), (0, 7), (9, 7), (3, 9), (7, 8)] 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)))
import datetime import random import numpy as np from planner.tcbs.plan import plan as plan_cbsext, generate_config from tools import load_map _map = load_map('planner/map.png') grid = np.repeat(_map[:, :, np.newaxis], 100, axis=2) landmarks = [(1, 1), (3, 1), (5, 1), (7, 1), (9, 1), (8, 5), (1, 6), (9, 7), (1, 8), (9, 9)] # input agents = [(1, 1), (3, 3), (5, 0), (7, 2), (9, 1), (5, 5), (0, 7), (9, 7), (3, 9), (7, 8)] 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)))
import numpy as np from tools import load_map from planner.greedy.greedy import plan_greedy from planner.eval.display import plot_inputs, plot_results _map = load_map('../map2.png') _map = _map[:, ::2] grid = np.repeat(_map[:, :, np.newaxis], 100, axis=2) # input agent_pos = [(0, 0), (1, 0), (2, 0)] 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), ((3, 4), (5, 8), 0)] idle_goals = [] fig = plot_inputs(agent_pos, idle_goals, jobs, grid, show=False) (res_agent_job, res_paths) = plan_greedy(agent_pos, jobs, grid, 'greedy.pkl') plot_results([], res_paths, res_agent_job, agent_pos, fig, grid, idle_goals, 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
def c(): _map = load_map('c.png') agent_pos = [(3, 3), (6, 5)] jobs = [((4, 3), (4, 5), 0), ((5, 5), (5, 3), 0)] eval(_map, agent_pos, jobs, 'c.pkl')
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, [], [],
import matplotlib.pyplot as plt import numpy as np from matplotlib import cm from mpl_toolkits.mplot3d import Axes3D from itertools import product from tools import load_map _ = Axes3D plt.style.use('bmh') grid = load_map('map.png') grid = np.repeat(grid[:, :, np.newaxis], 100, axis=2) grid = np.swapaxes(grid, 0, 1) f = plt.figure() ax = f.add_subplot(111, projection='3d') ax.set_facecolor('white') ax.set_xlim3d(0, grid.shape[0]) ax.set_ylim3d(0, grid.shape[1]) ax.set_zlim3d(0, grid.shape[2]) xx, yy = np.meshgrid(np.linspace(.5, grid.shape[0] + .5, grid.shape[0] * 50), np.linspace(.5, grid.shape[1] + .5, grid.shape[1] * 50)) img = np.zeros([xx.shape[0], yy.shape[1]]) for x, y in product(range(len(xx[0, :])), range(len(yy[:, 0]))): try: img[x, y] = grid[int(round(xx[0, x] - 1)), int(round(yy[y, 0] - 1)), 0] * -.001