def plotRobotMotion():
N = 100
control = np.array([1, 0.2])
pose = np.array([8, 15, 0])
deltaT = 5
plot.limit(0, 30, 0, 30)
plot.plotRobotPose(pose)
model = vm.VelocityMotionModel(0.001, 0.001, 0.05, 0.05, 0.001, 0.001)
poses = []
for i in range(N):
newPose = model.sampleNewPose(control, pose, deltaT)
poses.append(newPose)
plot.plotRobotPose(newPose)
plot.show()
for i in range(len(command)):
robot.motionUpdate(command[i], deltaT)
poses.append(robot.getPose())
for i in range(len(poses)):
robot.setPose(poses[i])
measurements.append(robot.measurementUpdate())
for i in range(len(measurements)):
fastSlam.measurementUpdate(measurements[i])
particle = fastSlam.getBestParticle()
fPositions = map(lambda f: f.getPosition(), particle.getFeatures())
plot.plotRobotPose(poses[i])
# plot.plotRobotPoses(map(lambda p: p.getPose(), fastSlam.getParticles()), style="ro")
plot.plotRobotPose(particle.getPose(), style="ro")
plot.plotFeatures(fPositions, 'bo')
plot.plotFeatures(landmarks)
plot.show()
fastSlam.resample()
if i < len(command):
fastSlam.motionUpdate(command[i], deltaT)
plot.plotRobotPoses(poses)
plot.plotRobotPoses(fastSlam.getBestParticle().getPath(), style="ro")
plot.plotFeatures(landmarks)
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()