def plotOccupancy():
data = plot.readBMPAsNumpyArray("../map/maze_map.bmp")
startPos = np.array([300, 1200])
# startPos = np.array([1200, 1800])
numRays = 8
paths = rc.raycastOmnidirection(data, startPos, numRays, True, 1000)
endPoints = rc.raycastOmnidirection(data, startPos, numRays, limit=3)
for i in range(len(paths)):
for j in range(len(paths[i])):
x = paths[i][j][0]
y = paths[i][j][1]
data[y, x] = 0
plot.plotOccupancyGrid(data, 0.01)
robot_width = 0.3
robot_length = 0.6
local_planner = rlp.RectangleLocalPlanner(obstacle_map, resolution,
step_size, robot_width,
robot_length)
config_sampler = rcs.RandomConfigSampler(local_planner)
num_samples = 20
# Uniformly sample configurations.
uniform_configs = []
for i in range(num_samples):
uniform_configs.append(config_sampler.uniform_collision_free_sample())
plot.plotOccupancyGrid(obstacle_map, resolution)
plot.plotRectangles(uniform_configs, robot_width, robot_length)
plot.show()
# Sample around obstacles.
sigmas = (0.1, 0.1, 0.5)
biased_configs = []
num_biased_samples = 20
for i in range(num_biased_samples):
new_config = config_sampler.sample_around_obstacle(sigmas, trials=200)
if new_config is not None:
biased_configs.append(new_config)
plot.plotOccupancyGrid(obstacle_map, resolution)
plot.plotRectangles(biased_configs, robot_width, robot_length)
plot.show()
bucket_interval=5.0)
# Compute the A star path.
print("start")
pr.enable()
planner.compute(start_config, end_config)
pr.disable()
path = planner.get_path()
all_path = planner.get_all_explored_path()
# Optimize the A star path.
path_points = planner.get_path_points(point_interval=1.0)
pr.enable()
optimized_points = path_optimizer.optimize_path(path_points)
pr.disable()
ps = pstats.Stats(pr).sort_stats('cumulative')
ps.print_stats()
uplt.plotOccupancyGrid(map, grid_resolution)
show([start_config, end_config], all_path, 0, 50, 0, 50)
uplt.show()
uplt.plotOccupancyGrid(map, grid_resolution)
show([start_config, end_config], path, 0, 50, 0, 50, point_interval=0.1)
uplt.show()
uplt.plotOccupancyGrid(map, grid_resolution)
show_path_points([start_config, end_config], optimized_points, 0, 50, 0,
50)
uplt.show()
z_max=limit)
robot = bot.Robot(grid, initialPose, motionModel, numRays, noiseSigma,
resolution, limit)
poses, measurements = generateData(robot, command1, initialPose)
shape = (int(grid.shape[0] / 10), int(grid.shape[1] / 10))
origin = [0, 0]
mapResolution = 0.1
locc = 1
lfree = -1
alpha = 0.5
beta = 2 * math.pi / numRays
map = occmap.OccupancyMap(shape[:], origin, mapResolution, locc, lfree,
limit, alpha, beta)
for i in range(len(poses)):
pose = poses[i]
measurement = measurements[i]
map.update(pose, measurement)
plt.figure()
probOccMap = map.generateProbabilityOccupancyGrid(probFree=True)
plt.imshow(probOccMap, cmap='gray', origin='lower')
plt.figure()
plot.plotOccupancyGrid(grid, resolution)
plot.plotRobotPoses(poses)
plot.show()