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 calc_cspace(self, mapdata):
"""
Calculates the C-Space, i.e., makes the obstacles in the map thicker.
:return [OccupancyGrid] The C-Space.
"""
rospy.loginfo("Calculating C-Space")
# Go through each cell in the occupancy grid
# Inflate the obstacles where necessary
padding = 2
newmap = deepcopy(mapdata)
# for each cell in mapdata
# if (cell is walkable) and (cell has an obstacle within padding)
# add padding in newmap at current cell
# input (x,y,pad)
# sx = min(max(0, x-pad), width)
# sy = min(max(0, y-pad), height)
# ex = max(min(width, x+pad), 0)
# ey = max(min(height, y+pad), 0)
# for each xx in range(sx,ex)
# for each yy in range(sy,ey)
# if cell(xx,yx) has obstacle
# return True
# return False
new_data = []
for item in mapdata.data:
new_data.append(item)
for h in range(mapdata.info.height - 1):
for w in range(mapdata.info.width - 1):
neighbors = ExpandMap.neighbors_of_8(mapdata, w, h)
if len(neighbors) < 8:
new_data[ExpandMap.grid_to_index(mapdata, w, h)] = 100
#Create a GridCells message and publish it
expanded_cells = GridCells()
expanded_cells.header.frame_id = "map"
expanded_cells.cell_width = mapdata.info.resolution
expanded_cells.cell_height = mapdata.info.resolution
expanded_cells.cells = []
for h in range(mapdata.info.height):
for w in range(mapdata.info.width):
index = ExpandMap.grid_to_index(mapdata, w, h)
if new_data[index] == 100:
msg_Point = Point()
world_point = PathPlanner.grid_to_world(mapdata, w, h)
msg_Point.x = world_point.x
msg_Point.y = world_point.y
msg_Point.z = 0
expanded_cells.cells.append(msg_Point)
self.expanded_cells.publish(expanded_cells)
## Return the C-space
#print("END")
#print(mapdata)
newmap.data = new_data
return newmap
def calc_cspace(self, mapdata, padding):
"""
Calculates the C-Space, i.e., makes the obstacles in the map thicker.
Publishes the list of cells that were added to the original map.
:param mapdata [OccupancyGrid] The map data.
:param padding [int] The number of cells around the obstacles.
:return [[int8]] The C-Space as a list of values from 0 to 100.
"""
rospy.loginfo("Calculating C-Space")
## Goes through each cell in the occupancy grid
## Inflates the obstacles where necessary based on padding argument
new_map = []
total_range = mapdata.info.width * mapdata.info.height
for p in range(0,padding):
copy = list(mapdata.data)
for i in range(0, total_range):
if mapdata.data[i] == 100:
#coordinates are found via indexing through occupancy grid.
#Index to grid is used to convert the index to useable grid coordinates
coordinates = PathPlanner.index_to_grid(mapdata, i)
pos_x = coordinates[0]
pos_y = coordinates[1]
neighbors = PathPlanner.neighbors_of_8(mapdata, pos_x, pos_y)
#iterates through neighbors list and changes all available surrounding neighbors to obstacles
for n in neighbors:
index_value = PathPlanner.grid_to_index(mapdata, n[0], n[1])
copy[index_value] = 100
world_pos = PathPlanner.grid_to_world(mapdata, n[0], n[1])
cell_point = Point()
cell_point.x = world_pos[0]
cell_point.y = world_pos[1]
cell_point.z = 0
new_map.append(cell_point)
mapdata.data = copy
return mapdata.data
def find_frontier(self, msg):
"""
Takes in map [OccupancyGrid] from service call
return: [Path] list of PoseStamped frontier centroids
"""
#cspace is calculated to find walkable area
msg.map.data = self.calc_cspace(msg.map, 2)
#variables are initialized
walkable_cells = []
cells_with_unknown_neighbors = []
centroids = []
distances = []
frontiers = []
frontier_locations = []
sorted_distances = []
total_range = msg.map.info.width * msg.map.info.height
mapdata = msg.map
#finds walkable cells in cspace
for i in range(0, total_range):
coordinate = PathPlanner.index_to_grid(mapdata, i)
pos_x = coordinate[0]
pos_y = coordinate[1]
if PathPlanner.is_cell_walkable(mapdata, pos_x, pos_y) == 1:
walkable_cells.append(coordinate)
# finds all cells that are neighboring an unknown cell(value = -1)
for c in walkable_cells:
neighbors = PathPlanner.unknown_neighbors_of_8(mapdata, c[0], c[1])
for n in neighbors:
if c not in cells_with_unknown_neighbors:
cells_with_unknown_neighbors.append(c)
# finds which cells in cells_with_unknown_neighbors neighbor each other
# and generates a list of those that touch
frontiers = Frontier.separate(cells_with_unknown_neighbors)
for f in frontiers:
if len(f) < 2:
frontiers.remove(f)
for f in frontiers:
#finds centroids of all contiguous frontier groups
centroid_value = Frontier.calc_centroid(f)
final = centroid_value
#finds the walkable cell that is closest to the centroid location
for k in walkable_cells:
neighbors = PathPlanner.neighbors_of_8(mapdata, k[0], k[1])
if len(neighbors) == 8:
distToCentroid = PathPlanner.euclidean_distance(k[0], k[1], final[0], final[1])
distances.append((distToCentroid, k))
#list of distances is sorted in descending order in terms of size
sorted_distances = sorted(distances)
for s in sorted_distances:
final = s[1]
world_coord = PathPlanner.grid_to_world(mapdata, final[0], final[1])
frontier_location = PoseStamped()
frontier_location.pose.position.x = world_coord[0]
frontier_location.pose.position.y = world_coord[1]
frontier_location.pose.position.z = 0.0
frontier_location.pose.orientation = Quaternion()
frontier_location.header.stamp = rospy.Time.now()
frontier_locations.append(frontier_location)
all_frontiers = Path()
all_frontiers.header.frame_id = 'map'
all_frontiers.poses = frontier_locations
return all_frontiers
