# 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)
