Ejemplo n.º 1
0
def step():
    global inp
    robot.count += 1
    inp = io.SensorInput(cheat=True)


    # discretize sonar readings
    # each element in discreteSonars 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
    discreteSonars = []
    for (sonarPose, distance) in zip(sonarDist.sonarPoses,inp.sonars):
        if NOISE_ON:
            distance = noiseModel.noisify(distance, gridSquareSize)
        sensorIndices = robot.map.pointToIndices(inp.odometry.transformPose(sonarPose).point())
        hitIndices = robot.map.pointToIndices(sonarDist.sonarHit(distance, sonarPose, inp.odometry))
        ray = util.lineIndices(sensorIndices, hitIndices)
        discreteSonars.append((distance, ray))


    # figure out where robot is
    startPoint = inp.odometry.point()
    startCell = robot.map.pointToIndices(startPoint)


    # if necessary, make new plan
    if robot.plan is None:
        print 'REPLANNING'
        robot.plan = search.ucSearch(robot.successors,
                              robot.map.pointToIndices(inp.odometry.point()),
                              lambda x: x == robot.goalIndices,
                              lambda x: 0)
    isGood = True
    for i in robot.plan:
        if not robot.map.isPassable(i) and isGood:
            print 'REPLANNING'
            robot.plan = search.ucSearch(robot.successors,
                                  robot.map.pointToIndices(inp.odometry.point()),
                                  lambda x: x == robot.goalIndices,
                                  lambda x: 0)
            isGood = False


    # graphics (draw robot's plan, robot's location, goalPoint)
    # do not change this block
    for w in robot.windows:
        w.redrawWorld()
    robot.windows[-1].markCells(robot.plan,'blue')
    robot.windows[-1].markCell(robot.map.pointToIndices(inp.odometry.point()),'gold')
    robot.windows[-1].markCell(robot.map.pointToIndices(goalPoint),'green')


    # update map
    for (d,cells) in discreteSonars:
        if d != 5.0:
            robot.map.sonarHit(cells[-1])


    # if we are within 0.1m of the goal point, we are done!
    if startPoint.distance(goalPoint) <= 0.1:
        io.Action(fvel=0,rvel=0).execute()
        code = checkoff.generate_code(globals())
        raise Exception('Goal Reached!\n\n%s' % code)

    # otherwise, figure out where to go, and drive there
    destinationPoint = robot.map.indicesToPoint(robot.plan[0])
    while startPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1:
        robot.plan.pop(0)
        destinationPoint = robot.map.indicesToPoint(robot.plan[0])

    currentAngle = inp.odometry.theta
    angleDiff = startPoint.angleTo(destinationPoint)-currentAngle
    angleDiff = util.fixAnglePlusMinusPi(angleDiff)
    fv = rv = 0
    if startPoint.distance(destinationPoint) > 0.1:
        if abs(angleDiff) > 0.1:
            rv = angleDiff
        else:
            fv = 1.5*startPoint.distance(destinationPoint)
    io.setForward(fv)
    io.setRotational(rv)


    # render the drawing windows
    # do not change this block
    for w in robot.windows:
        w.render()
Ejemplo n.º 2
0
def step():
    robot.count += 1
    inp = io.SensorInput(cheat=True)
    for c in ('orange','cyan','blue','red'):
        robot.map.clearColor(c)

    # discretize sonar readings
    # each element in discreteSonars 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
    discreteSonars = []
    for (sonarPose,d) in zip(sonarDist.sonarPoses,inp.sonars):
        if NOISE_ON:
            r = random.random()
            if d != 5:
                if r > .99:
                    d = 5
                elif r > .97:
                    d = random.uniform(gridSquareSize,d)
            else:
                if r > .98:
                    d = random.uniform(gridSquareSize,1.5)
        discreteSonars.append((d,util.lineIndices(robot.map.pointToIndices(inp.odometry.transformPose(sonarPose)), robot.map.pointToIndices(sonarDist.sonarHit(d, sonarPose, inp.odometry)))))
    
    # update map
    for (d,cells) in discreteSonars:
        if d != 5:
            robot.map.set(cells[-1])
            for index in range(0, len(cells)-1):
                robot.map.clear(cells[index])
        

    # if necessary, make new plan
    if robot.plan is None or not all(robot.map.isPassable(i) for i in robot.plan):
        print 'REPLANNING'
        robot.plan = search.ucSearch(search.MazeSearchNode(robot.map,
                              robot.map.pointToIndices(inp.odometry.point()),None,0), 
                              lambda x: x == robot.map.pointToIndices(goalPoint), 
                              lambda x: 0)
        

    # graphics (draw robot's plan, robot's location, goalPoint)
    robot.map.markCells(robot.plan,'blue')
    robot.map.markCell(robot.map.pointToIndices(inp.odometry.point()),'red')
    robot.map.markCell(robot.map.pointToIndices(goalPoint),'green')

    # move to target point (similar to driving task in DL2)
    currentPoint = inp.odometry.point()
    currentAngle = inp.odometry.theta
    destinationPoint = robot.map.indicesToPoint(robot.plan[0])
    while currentPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1:
        robot.plan.pop(0)
        destinationPoint = robot.map.indicesToPoint(robot.plan[0])

    if not currentPoint.isNear(destinationPoint,0.1):
        angle = util.fixAnglePlusMinusPi(currentPoint.angleTo(destinationPoint)-currentAngle)
        if abs(angle)<0.1:
            #if close enough to pointing, use proportional control on forward velocity
            fv = 2*currentPoint.distance(destinationPoint)
            rv = 0
        else:
            #otherwise, use proportional control on angular velocity
            fv = 0
            rv = 2*angle
    else:
        raise Exception, 'Goal Reached!'

    robot.map.update()
    io.Action(fvel=fv,rvel=rv).execute()
Ejemplo n.º 3
0
def step():
	robot.count += 1
	inp = io.SensorInput(cheat=True)
	for c in ('orange','cyan','blue','red'):
		robot.map.clearColor(c)

	# discretize sonar readings
	# each element in discreteSonars 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
	discreteSonars = []
	for (sonarPose,d) in zip(sonarDist.sonarPoses,inp.sonars):
		if NOISE_ON:
			d = noiseModel.noisify(d,gridSquareSize)
		discreteSonars.append((d,util.lineIndices(robot.map.pointToIndices(inp.odometry.transformPose(sonarPose)), robot.map.pointToIndices(sonarDist.sonarHit(d, sonarPose, inp.odometry)))))
	
	# update map
	for (d,cells) in discreteSonars:
		# update probabilities
		if(d<=1.5):
			robot.mentalmap[cells[-1]].update(True)
			for c in cells[:-1]:
				robot.mentalmap[c].update(False)
		else:
			for i in range(0, len(cells)/4):
				robot.mentalmap[cells[i]].update(False)

		# update the map
		for c in cells:
			belief = robot.mentalmap[c].belief
			state = belief.maxProbElt()
			if(state):
				robot.map.sonarHit(c)
			else:
				robot.map.sonarPass(c)

	# null the path if necessary
	if not robot.plan is None:
		for pt in robot.plan:
			if(not robot.map.isPassable(pt)):
				robot.plan = None
				break

	if robot.plan is None:
		print 'REPLANNING'
		robot.dirty = True
		robot.plan = search.ucSearch(search.MazeSearchNode(robot.map,
							  robot.map.pointToIndices(inp.odometry.point()),None,0), 
							  lambda x: x == robot.map.pointToIndices(goalPoint), 
							  lambda x: 0)

	# graphics (draw robot's plan, robot's location, goalPoint)
	# do not change this block
	robot.map.markCells(robot.plan,'blue')
	robot.map.markCell(robot.map.pointToIndices(inp.odometry.point()),'red')
	robot.map.markCell(robot.map.pointToIndices(goalPoint),'green')

	# move to target point (similar to driving task in DL2)
	currentPoint = inp.odometry.point()
	currentAngle = inp.odometry.theta

	destinationPoint = robot.map.indicesToPoint(robot.plan[0])
	
	# check if we are ahead in the path
	if(robot.dirty):
		for i in range(0, len(robot.plan)):
			if(robot.map.pointToIndices(currentPoint)==robot.plan[i]):
				destination = robot.map.indicesToPoint(robot.plan[i+1])

	while currentPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1:
		robot.plan.pop(0)
		destinationPoint = robot.map.indicesToPoint(robot.plan[0])

	if not currentPoint.isNear(destinationPoint,0.1):
		angle = util.fixAnglePlusMinusPi(currentPoint.angleTo(destinationPoint)-currentAngle)
		if abs(angle)<0.1:
			#if close enough to pointing, use proportional control on forward velocity
			fv = SPEED*currentPoint.distance(destinationPoint)
			rv = 0
		else:
			#otherwise, use proportional control on angular velocity
			fv = 0
			rv = ANG_SPEED*angle
	else:
		t = time.time() - robot.startTime;o=inp.odometry.xytTuple();w=THE_WORLD[:1]+[THE_WORLD[1].xyTuple()]+THE_WORLD[2:];n=NOISE_ON
		raise Exception, 'Goal Reached!\n\n%s' % e(repr((t,o,w,n)))
	robot.map.update()
	io.Action(fvel=fv,rvel=rv).execute()

	robot.dirty = True
Ejemplo n.º 4
0
def step():
    global inp
    robot.count += 1
    inp = io.SensorInput(cheat=True)
    
    for c in ('orange','cyan','blue','red','gold'):
        robot.map.clearColor(c)

    if robot.map.doHeatMap:
        robot.map.heatMap()
        
    # discretize sonar readings
    # each element in discreteSonars 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
    discreteSonars = []
    for (sonarPose,d) in zip(sonarDist.sonarPoses,inp.sonars):
        if NOISE_ON:
            d = noiseModel.noisify(d,gridSquareSize)
        if d < 1.5:
            discreteSonars.append((d,util.lineIndices(robot.map.pointToIndices(inp.odometry.transformPose(sonarPose)), robot.map.pointToIndices(sonarDist.sonarHit(d, sonarPose, inp.odometry)))))


    # update map
    for (d,cells) in discreteSonars:
        robot.map.sonarHit(cells[-1])

    # if necessary, make new plan
    secondary_sonars = discreteSonars[:3] + discreteSonars[5:]
    if robot.plan is None or tooCloseToWall(discreteSonars[3:5], secondary_sonars):
        print 'REPLANNING'
        robot.plan = search.ucSearch(search.MazeSearchNode(robot.map,
                              robot.map.pointToIndices(inp.odometry.point()),None,0), 
                              lambda x: x == robot.map.pointToIndices(goalPoint), 
                              lambda x: 0)

    # graphics (draw robot's plan, robot's location, goalPoint)
    # do not change this block
    if robot.map.showPassable:
        robot.map.markNotPassable()
    if robot.plan is not None:
        robot.map.markCells(robot.plan,'blue')
    robot.map.markCell(robot.map.pointToIndices(inp.odometry.point()),'gold')
    robot.map.markCell(robot.map.pointToIndices(goalPoint),'green')

    # move to target point (similar to driving task in DL2)
    currentPoint = inp.odometry.point()
    currentAngle = inp.odometry.theta
    destinationPoint = robot.map.indicesToPoint(robot.plan[0])
    while currentPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1:
        robot.plan.pop(0)
        destinationPoint = robot.map.indicesToPoint(robot.plan[0])

    if not currentPoint.isNear(destinationPoint,0.1):
        angle = util.fixAnglePlusMinusPi(currentPoint.angleTo(destinationPoint)-currentAngle)
        if abs(angle)<0.1:
            #if close enough to pointing, use proportional control on forward velocity
            fv = 2*currentPoint.distance(destinationPoint)
            rv = 0
        else:
            #otherwise, use proportional control on angular velocity
            fv = 0
            rv = 2*angle
    else:
        raise Exception,'Goal Reached!\n\n%s' % checkoff.generate_code(globals())
            
    robot.map.render()
    io.Action(fvel=fv,rvel=rv).execute()