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()