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
