def main(seqs, dataset_folder, inputs_filename, heuristic_name, version,
outputs_folder, outputs_tag, times, rounds, debug, optimize):
outputs_folder = outputs_folder + os.sep + outputs_tag + os.sep + heuristic_name + os.sep + version
os.makedirs(outputs_folder) if not os.path.isdir(outputs_folder) else None
rrt_star = BiRRTStar().set_vehicle(contour(), 0.3, 0.2)
for i, seq in enumerate(
seqs): # read_seqs(dataset_folder, inputs_filename)
print('Processing Scene: {} ({} of {})'.format(seq, i + 1, len(seqs)))
heuristic = read_heuristic('./predictions' + os.sep + outputs_tag, seq,
heuristic_name)
source, target = read_task(dataset_folder + os.sep + 'scenes', seq)
start = center2rear(deepcopy(source)).gcs2lcs(source.state)
goal = center2rear(deepcopy(target)).gcs2lcs(source.state)
grid_ori = deepcopy(source).gcs2lcs(source.state)
grid_map, grid_res = read_grid(dataset_folder + os.sep + 'scenes',
seq), 0.1
rrt_star.debug = debug
Debugger.plan_hist = []
past = time.time()
for r in range(rounds):
rrt_star.preset(start, goal, grid_map, grid_res, grid_ori, 255,
heuristic)
rrt_star.planning(times,
repeat=100,
optimize=optimize,
debug=debug)
if rrt_star.x_best.fu < np.inf:
path = rrt_star.path()
taj = rrt_star.trajectory(a_cc=3, v_max=10, res=0.2)
np.savetxt('{}/{}_path.txt'.format(outputs_folder, seq),
[p.state for p in path],
delimiter=',')
taj = [t.lcs2gcs(source.state) for t in taj]
motion = [(t.state[0], t.state[1], t.state[2], t.k, t.v)
for t in taj]
print motion
np.savetxt('{}/{}_trajectory.txt'.format(outputs_folder, seq),
motion,
delimiter=',')
print(' Runtime: {}'.format(time.time() - past))
Debugger().save_hist(outputs_folder + os.sep + str(seq) +
'_summary.txt')
def main(dataset_folder, inputs_filename, heuristic_name, outputs_folder,
outputs_tag, times, rounds, debug, optimize):
outputs_folder = outputs_folder + os.sep + outputs_tag + os.sep + heuristic_name
rrt_star = BiRRTStar().set_vehicle(contour(), 0.3, 0.2)
seqs = read_seqs(dataset_folder, inputs_filename)
for i, seq in enumerate([2384
]): # read_seqs(dataset_folder, inputs_filename)
print('Processing Scene: {} ({} of {})'.format(seq, i + 1, len(seqs)))
set_plot(True)
heuristic = read_heuristic('./predictions' + os.sep + outputs_tag, seq,
heuristic_name)
source, target = read_task(dataset_folder + os.sep + 'scenes', seq)
start = center2rear(deepcopy(source)).gcs2lcs(source.state)
goal = center2rear(deepcopy(target)).gcs2lcs(source.state)
grid_ori = deepcopy(source).gcs2lcs(source.state)
grid_map, grid_res = read_grid(dataset_folder + os.sep + 'scenes',
seq), 0.1
print heuristic[0][0]
Debugger.plot_grid(grid_map, grid_res)
Debugger.plot_heuristic(heuristic, alpha=1.0, r=5.0)
Debugger.plot_heuristic([(heuristic[0][0], ((0.0, 2.), (0.0, 2.0),
(0.0, 0.524)))],
color='C0',
r=4.0,
alpha=1.0)
Debugger.plot_heuristic([(heuristic[0][0], ((0.0, 4.), (0.0, 4.0),
(0.0, 1.047)))],
color='C1',
r=3.0,
alpha=1.0)
Debugger.plot_heuristic([(heuristic[0][0], ((0.0, 1.), (0.0, 1.0),
(0.0, 0.262)))],
color='C3',
r=6.0,
alpha=1.0)
rrt_star.debug = debug
Debugger.plan_hist = []
Debugger.breaker(' ')
past = time.time()
for r in range(rounds):
rrt_star.preset(start, goal, grid_map, grid_res, grid_ori, 255,
heuristic)
rrt_star.planning(times,
repeat=100,
optimize=optimize,
debug=debug)
print(' Runtime: {}'.format(time.time() - past))
os.makedirs(
outputs_folder) if not os.path.isdir(outputs_folder) else None
Debugger().save_hist(outputs_folder + os.sep + str(seq) +
'_summary.txt')
def plot_task(grid_map, grid_res, heuristic, start, goal):
Debugger.plot_grid(grid_map, grid_res)
Debugger.plot_heuristic(heuristic) if heuristic else None
Debugger().plot_polygon(transform(contour(), start.state), color='y')
Debugger().plot_polygon(transform(contour(), goal.state), color='y')
plt.draw()
def plot_task(grid_map, grid_res, start, goal):
Debugger.plot_grid(grid_map, grid_res)
plot_state2(start)
plot_state2(goal)
plt.draw()
def calculate_performance(seqs, predictor, dataset_folder, inputs_filename,
prediction_folder, planning_folder):
threshold = 0.5
rho = 5.0
for i, seq in enumerate(seqs): # enumerate(seqs)
print('Evaluate Scene: {} ({} of {})'.format(seq, i + 1, len(seqs)))
inference = read_inference(prediction_folder, seq, predictor)
label = read_label(dataset_folder, seq)
start, goal = read_task2(dataset_folder, seq)
pred = build_path(start, inference, goal, threshold)
true = build_path(start, label, goal, threshold)
pred_length = calculate_path_length(pred, rho=rho)
true_length = calculate_path_length(true, rho=rho)
optimal_length = calculate_path_length([start, goal], rho=rho)
fontsize = 70
ax1 = new_figure(y_label='$\widehat{\mathrm{mLOP}}$',
x_label='time[s]',
fontsize=fontsize)
labels = ['Optimal', 'Greedy', 'Normal']
colors = ['r', 'b', 'g']
for j, version in enumerate(['optimal', 'greedy']):
summary = read_yips_planning_summary(planning_folder, seq,
predictor + os.sep + version)
summaries = np.split(summary, 100)
summary = np.mean(summaries, axis=0)
lens = summary[:, 2]
times = summary[:, 1]
samples = summary[:, 0]
normalized_lens = lens / optimal_length
ax1.plot(times,
normalized_lens,
lw=12,
c=colors[j],
zorder=100 - j,
label=labels[j])
print true_length, optimal_length
ax1.set_ylim([0.0, 1.7])
ax1.set_yticks([0.5, 1.0, 1.5])
ax1.set_xlim([0, .5])
ax1.set_xticks([0, .2, 0.4])
ax1.hlines(true_length / optimal_length,
0,
2,
lw=12,
color='b',
zorder=20,
linestyles='dotted',
label='Label')
ax1.hlines(optimal_length / optimal_length,
0,
2,
lw=12,
color='C1',
zorder=15,
linestyles='-',
label='Geodesic')
ax1.legend(prop={'size': fontsize - 12},
loc=4,
frameon=True,
ncol=2,
handlelength=1.5)
plt.draw()
plt.show()
Debugger.breaker('')