def on_stop():
for i in range(len(robot.slime_x)):
robot.window.drawPoint(robot.slime_x[i], robot.slime_y[i], 'red')
if worldname == 'bigEmptyWorld' and sys.exc_info() == (None, None, None):
code = checkoff.generate_code(globals())
if isinstance(code, bytes):
code = code.decode()
print('Code for Driving in Tutor:\n%s' % code)
def on_stop():
if not REAL_ROBOT:
code = checkoff.generate_code(globals())
if isinstance(code, bytes):
code = code.decode()
print("Hex Code for Tutor:\n%s" % code, file=sys.stderr)
p = PlotWindow()
p.plot(robot.probMeasures)
p.axes[0].set_ylim([0.0,1.0])
def on_stop():
if not REAL_ROBOT:
code = checkoff.generate_code(globals())
if isinstance(code, bytes):
code = code.decode()
print("Hex Code for Tutor:\n%s" % code, file=sys.stderr)
p = PlotWindow()
p.plot(robot.probMeasures)
p.axes[0].set_ylim([0.0, 1.0])
def on_step():
global inp
robot.count += 1
inp = io.SensorInput(cheat=True)
# discretize sonar readings
# each element in discrete_sonars is a tuple (d, cells)
# d is the distance measured by the sonar
# cells is a list of grid cells (r,c) between the sonar and the point d meters away
discrete_sonars = []
for (sonar_pose, distance) in zip(sonarDist.sonar_poses,inp.sonars):
if NOISE_ON:
distance = noiseModel.noisify(distance, grid_square_size)
if distance >= 5:
sensor_indices = robot.map.point_to_indices(inp.odometry.transformPose(sonar_pose).point())
hit_indices = robot.map.point_to_indices(sonarDist.sonar_hit(1, sonar_pose, inp.odometry))
ray = util.line_indices(sensor_indices, hit_indices)
discrete_sonars.append((distance, ray))
continue
sensor_indices = robot.map.point_to_indices(inp.odometry.transformPose(sonar_pose).point())
hit_indices = robot.map.point_to_indices(sonarDist.sonar_hit(distance, sonar_pose, inp.odometry))
ray = util.line_indices(sensor_indices, hit_indices)
discrete_sonars.append((distance, ray))
# figure out where robot is
start_point = inp.odometry.point()
startCell = robot.map.point_to_indices(start_point)
def plan_wont_work():
for point in robot.plan:
if not robot.map.is_passable(point):
return True
return False
# if necessary, make new plan
if robot.plan is None or plan_wont_work(): #discrete_sonars[3][0] < 0.3: ###### or if the robot is about to crash into a wall
print('REPLANNING')
robot.plan = search.uniform_cost_search(robot.successors,
robot.map.point_to_indices(inp.odometry.point()),
lambda x: x == robot.goalIndices,
lambda x: 0)
# make the path less jittery
#print(robot.plan)
to_remove = []
if robot.plan is not None:
for i in range(len(robot.plan)-2):
line = util.LineSeg(util.Point(robot.plan[i][0], robot.plan[i][1]), util.Point(robot.plan[i+1][0], robot.plan[i+1][1]))
if line.dist_to_point(util.Point(robot.plan[i+2][0], robot.plan[i+2][1])) < 0.1:
to_remove.append(robot.plan[i+1])
print(to_remove)
for i in to_remove:
robot.plan.remove(i)
#print(robot.plan)
#print()
# graphics (draw robot's plan, robot's location, goal_point)
# do not change this block
for w in robot.windows:
w.redraw_world()
robot.windows[-1].mark_cells(robot.plan,'blue')
robot.windows[-1].mark_cell(robot.map.point_to_indices(inp.odometry.point()),'gold')
robot.windows[-1].mark_cell(robot.map.point_to_indices(goal_point),'green')
# update map
for (d,cells) in discrete_sonars:
#print(cells)
if d < 5:
robot.map.sonar_hit(cells[-1])
for i in range(len(cells)-1):
robot.map.sonar_pass(cells[i])
# if we are within 0.1m of the goal point, we are done!
if start_point.distance(goal_point) <= 0.1:
io.Action(fvel=0,rvel=0).execute()
code = checkoff.generate_code(globals())
if isinstance(code, bytes):
code = code.decode()
raise Exception('Goal Reached!\n\n%s' % code)
# otherwise, figure out where to go, and drive there
destination_point = robot.map.indices_to_point(robot.plan[0])
while start_point.isNear(destination_point,0.1) and len(robot.plan)>1:
robot.plan.pop(0)
destination_point = robot.map.indices_to_point(robot.plan[0])
currentAngle = inp.odometry.theta
angleDiff = start_point.angleTo(destination_point)-currentAngle
angleDiff = util.fix_angle_plus_minus_pi(angleDiff)
fv = rv = 0
if start_point.distance(destination_point) > 0.1:
if abs(angleDiff) > 0.1:
rv = 4*angleDiff
else:
fv = 4*start_point.distance(destination_point)
io.set_forward(fv)
io.set_rotational(rv)
# render the drawing windows
# do not change this block
for w in robot.windows:
w.render()
