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