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
