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)
