def run_benchmark(database_folders=[], num_envs=1):
global env_params, lattice_params, cost_fn, heuristic_fn, lattice, planner, start_n, goal_n
# h_weight_list = range(1, 100, 10)
# h_weight_list = [0] + h_weight_list
h_weight_list = [0]
e = Env2D()
print('Running benchmark')
for folder in database_folders:
results = defaultdict(list)
for i in range(num_envs):
curr_env_file = os.path.join(os.path.abspath(folder),
str(i) + '.png')
e.initialize(curr_env_file, env_params)
for h_weight in h_weight_list:
prob_params = {'heuristic_weight': h_weight}
prob = PlanningProblem(prob_params)
prob.initialize(e,
lattice,
cost_fn,
heuristic_fn,
start_n,
goal_n,
visualize=True)
planner.initialize(prob)
path, path_cost, num_expansions, plan_time, came_from, cost_so_far, c_obs = planner.plan(
)
results[h_weight].append((num_expansions, plan_time))
planner.clear_planner() #clear the planner in the end
env_name = os.path.split(os.path.split(os.path.abspath(folder))[0])[1]
output_file_1 = "astar_2d_benchmark.json"
json.dump(results,
open(
os.path.join(
os.path.abspath("../benchmark_results/astar/" +
env_name), output_file_1), 'w'),
sort_keys=True)
output_file_2 = "astar_2d_enchmark_averaged.json"
#Calculate average expansions
avg_results = defaultdict(list)
for k, v in results.iteritems():
avg_expansions = 0
avg_time = 0
for exp, t in v:
avg_expansions += exp
avg_time += t
avg_expansions /= num_envs
avg_time /= num_envs
avg_results[k] = (avg_expansions, avg_time)
json.dump(avg_results,
open(
os.path.join(
os.path.abspath("../benchmark_results/astar/" +
env_name), output_file_2), 'w'),
sort_keys=True)
lattice_params['path_resolution'] = 1 #Resolution for defining edges and doing collision checking (in meters)
l = XYAnalyticLattice(lattice_params)
#Step 4: Create cost and heuristic objects
cost_fn = PathLengthNoAng() #Penalize length of path
heuristic_fn = EuclideanHeuristicNoAng()
#(Additionally, you can precalculate edges and costs on lattice for speed-ups)
l.precalc_costs(cost_fn) #useful when lattice remains same across problems
#Step 5: Create a planning problem
prob_params = {'heuristic_weight': 1.0}
start_n = l.state_to_node(start)
goal_n = l.state_to_node(goal)
prob = PlanningProblem(prob_params)
prob.initialize(e, l, cost_fn, heuristic_fn, start_n, goal_n, visualize=visualize)
#Step 6: Create Planner object and ask it to solve the planning problem
planner = GreedyPlanner()
planner.initialize(prob)
path, path_cost, num_expansions, plan_time, came_from, cost_so_far, c_obs = planner.plan()
print('Path: ', path)
print('Path Cost: ', path_cost)
print('Number of Expansions: ', num_expansions)
print('Time taken: ', plan_time)
e.initialize_plot(start, goal)
e.plot_path(path, 'solid', 'red', 3)
plt.show()