def rvizObstacles(w_map): global obstacles_pub obstacles_GC = GridCells() obstacles_GC.cell_width = w_map.info.resolution obstacles_GC.cell_height = w_map.info.resolution obstacles_GC.cells = [] obstacle_pts = [] expanded_pts = set([]) e_o_i = set([]) for index, node in enumerate(w_map.data): if node > 50: obstacle_pts.append(index) for index in obstacle_pts: for i in range(-4, 5): for j in range(-4, 5): point = i_to_p(index, w_map.info.width) point.x += i point.y += j e_o_i.add(p_to_i(point, w_map.info.width)) expanded_pts.add(lab_4.gridToWorld(point, w_map)) obstacles_GC.cells = list(expanded_pts) obstacles_GC.header.frame_id = 'map' obstacles_pub.publish(obstacles_GC) return list(e_o_i)
def rvizFrontier(cell_list, w_map): global frontier_pub frontier_GC = GridCells() frontier_GC.cell_width = w_map.info.resolution frontier_GC.cell_height = w_map.info.resolution frontier_GC.cells = [] for cell in cell_list: frontier_GC.cells.append(lab_4.gridToWorld(i_to_p(cell, w_map.info.width), w_map)) frontier_GC.header.frame_id = 'map' frontier_pub.publish(frontier_GC)
def rvizUnexplored(cell_list, w_map): global unexplored_pub unexplored_GC = GridCells() unexplored_GC.cell_width = w_map.info.resolution unexplored_GC.cell_height = w_map.info.resolution unexplored_GC.cells = [] for cell in cell_list: unexplored_GC.cells.append(lab_4.gridToWorld(i_to_p(cell, w_map.info.width), w_map)) unexplored_GC.header.frame_id = 'map' unexplored_pub.publish(unexplored_GC)
def rvizExpanded(cell_list, w_map): global expanded_pub expanded_GC = GridCells() expanded_GC.cell_width = w_map.info.resolution expanded_GC.cell_height = w_map.info.resolution expanded_GC.cells = [] for cell in cell_list: expanded_GC.cells.append(lab_4.gridToWorld(i_to_p(cell, w_map.info.width), w_map)) expanded_GC.header.frame_id = 'map' expanded_pub.publish(expanded_GC)