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
mask = np.logical_and(obstacle_mask, new_mask)
coverage_grid.draw()
pr.step()
# End Simulations
pr.stop()
pr.shutdown()
