def generate_paths(self, cost_map):
        problem_valid = False

        # Choose a goal position
        num_alternatives = len(self.possible_goals)
        goal_position = self.possible_goals[random.randint(0, num_alternatives-1)]

        while not problem_valid:
            # Trying to generate a new problem
            start_position = (random.randint(0, self.height - 1), random.randint(0, self.width - 1))
            # If the start happen to be within an obstacle, we discard them and
            # try new samples
            if cost_map.is_occupied(start_position[0], start_position[1]):
                continue
            if start_position == goal_position:
                continue
            try:
                path_planner = PathPlanner(cost_map)
                dijkstra_path, cost = path_planner.dijkstra(start_position, goal_position)
                greedy_path, cost = path_planner.greedy(start_position, goal_position)
                a_star_path, cost = path_planner.a_star(start_position, goal_position)

                problem_valid = True

            except AttributeError:
                # In case there is no valid path
                continue

        # print(start_position, goal_position)
        # plt.matshow(cost_map.grid)
        # plt.plot(start_position[1], start_position[0], 'g*', markersize=8)
        # plt.plot(goal_position[1], goal_position[0], 'rx', markersize=8)
        # title = str(start_position) + ", " + str(goal_position)
        # plt.title(title)
        # plt.show()

        return [dijkstra_path, greedy_path, a_star_path]
Esempio n. 2
0
        start_position = (random.randint(0, HEIGHT - 1),
                          random.randint(0, WIDTH - 1))
        goal_position = (random.randint(0, HEIGHT - 1),
                         random.randint(0, WIDTH - 1))
        # If the start or goal positions happen to be within an obstacle, we discard them and
        # try new samples
        if cost_map.is_occupied(start_position[0], start_position[1]):
            continue
        if cost_map.is_occupied(goal_position[0], goal_position[1]):
            continue
        if start_position == goal_position:
            continue
        problem_valid = True
    tic = time.time()
    if algorithm == 'dijkstra':
        path, cost = path_planner.dijkstra(start_position, goal_position)
    elif algorithm == 'greedy':
        path, cost = path_planner.greedy(start_position, goal_position)
    else:
        path, cost = path_planner.a_star(start_position, goal_position)
    # if path is not None and len(path) > 0:
    path_found = True
    toc = time.time()
    times[i] = toc - tic
    costs[i] = cost
    plot_path(cost_map, start_position, goal_position, path,
              '%s_%d' % (algorithm, i), save_fig, show_fig, fig_format)

# Print Monte Carlo statistics
print(r'Compute time: mean: {0}, std: {1}'.format(np.mean(times),
                                                  np.std(times)))