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