def wall(p1, p2): global grid c1, c2 = point_to_indices(p1), point_to_indices(p2) indices = line_indices(c1, c2) for (r, c) in indices: grid[r][c] = '#'
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()