def execute_rendezvous( self, master_coords, drone_coords ): # calculates rendezvous path and sets nav goal for robots at the provided poses if self.executing_rendezvous or not self.map_initialized: return False # init and run the path planner path_planner = PathPlanner(self.occupancy_grid, self.map_width, self.map_height, self.map_resolution, self.map_origin) path_planner.set_start(master_coords) path_planner.set_goal(drone_coords) rendezvous_path = path_planner.a_star() # abort if no path found if len(rendezvous_path) == 0: return False # parse the result and start nav to goal for both robots grid_goal = rendezvous_path[int(math.floor(len(rendezvous_path) / 2))] world_goal = path_planner.grid_to_world(grid_goal) print 'executing rendezvous at ' + str(world_goal) self.master_node.start_navigation( world_goal[0], world_goal[1], 0, (master_coords[0], master_coords[1], 0)) self.drone_node.start_navigation(world_goal[0], world_goal[1], 0, (drone_coords[0], drone_coords[1], 0)) self.executing_rendezvous = True return True
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]
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))) if not (inf in costs): print(r'Cost: mean: {0}, std: {1}'.format(np.mean(costs), np.std(costs)))