del planner
ccd = CCRA(occ_grid, block_size_x, block_size_y)
path = ccd.getPath((cur_pos.x, cur_pos.y))
# For visualization
coverage_grid = OccupancyGrid()
setupOccGrid(coverage_grid, vision_sensor)
obstacle_mask = coverage_grid.grid == 0
mask = obstacle_mask
# Set obstacles to -1 for coverage calculations
coverage_grid.grid *= -1
coverage_grid.setup_drawing()
for (p_x, p_y), layer in path:
# Move the robot along the path
wx, wy = occ_grid.mapToWorld(p_x, p_y)
pose = Pose(wx, wy, -layer * math.pi / 8)
set2DPose(robot, pose)
# Visualization
new_coverage = OccupancyGrid()
setupOccGrid(new_coverage, vision_sensor)
prev_grid = np.array(coverage_grid.grid, copy=True)
coverage_grid.grid[mask] += new_coverage.grid[mask]
new_mask = new_coverage.grid == 0
