# 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 mask = np.logical_and(obstacle_mask, new_mask) coverage_grid.draw() pr.step()
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() # TODO: Rotational interpolation for p in path: # Move the robot along the path wx, wy = occ_grid.mapToWorld(p[0], p[1]) pose = Pose(wx, wy, math.radians(p[2] * 90)) # print(wx, wy) set2DPose(robot, pose) pr.step() # time.sleep(0.01) # 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)