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