Python NavHelper类代码示例

def avoidRight(nav):
    if nav.firstFrame():
        avoidDest = RelRobotLocation(0, -25, 0)
        helper.setOdometryDestination(nav, avoidDest)
        return nav.stay()

    return Transition.getNextState(nav, avoidRight)

def walking(nav):
    """
    State to be used for velocity walking.
    """
    helper.setSpeed(nav, walking.speeds)

    return Transition.getNextState(nav, walking)

def dodge(nav):
    if nav.firstFrame():
        nav.dodging = True

        if nav.brain.ball.vis.frames_off < 5:
            nav.brain.tracker.trackBall()
        else:
            nav.brain.tracker.trackObstacle(dodge.obstaclePosition)

        ## SET UP the dodge direction based on where the obstacle is
        # if directly in front of us, move back and to one side based on
        # where the goToPosition dest is
        dodge.speed = Navigator.BRISK_SPEED

        obstacleInfo = constants.OBS_DICT[dodge.obstaclePosition]
        helper.createAndSendWalkVector(nav, 
                                        dodge.speed*obstacleInfo[0], 
                                        dodge.speed*obstacleInfo[1], 
                                        0)
        print "Dodging ", obstacleInfo[2], " Obstacle"

    if navTrans.doneDodging(nav):
        nav.dodging = False
        nav.brain.tracker.repeatBasicPan()
        return nav.goLater('briefStand')

    return nav.stay()

def avoidRightObstacle(nav):
    """
    dodges left if we only detect something to the left of us
    """
    if nav.firstFrame():
        nav.doneAvoidingCounter = 0
        nav.printf(nav.brain.sonar)
        nav.printf("Avoid by left dodge");
        helper.setSpeed(nav, (0, constants.DODGE_LEFT_SPEED, 0))

    avoidLeft = navTrans.shouldAvoidObstacleLeft(nav)
    avoidRight = navTrans.shouldAvoidObstacleRight(nav)

    if (avoidLeft and avoidRight):
        return nav.goLater('avoidFrontObstacle')
    elif avoidLeft:
        return nav.goLater('avoidLeftObstacle')
    elif avoidRight:
        nav.doneAvoidingCounter -= 1
        nav.doneAvoidingCounter = max(0, nav.doneAvoidingCounter)
        return nav.stay()
    else:
        nav.doneAvoidingCounter += 1

    if nav.doneAvoidingCounter > constants.DONE_AVOIDING_FRAMES_THRESH:
        nav.shouldAvoidObstacleRight = 0
        nav.shouldAvoidObstacleLeft = 0
        return nav.goLater(nav.preAvoidState)

    return nav.stay()

def goToPosition(nav):
    """
    Go to a position set in the navigator. General go to state.  Goes
    towards a location on the field stored in dest.
    The location can be a RobotLocation, Location, RelRobotLocation, RelLocation
    Absolute locations get transformed to relative locations based on current loc
    For relative locations we use our bearing to that point as the heading
    """

    relDest = helper.getRelativeDestination(nav.brain.my, goToPosition.dest)
    goToPosition.deltaDest = relDest # cache it for later use

#    if nav.counter % 10 is 0:
#        print "going to " + str(relDest)
#        print "ball is at {0}, {1}, {2} ".format(nav.brain.ball.loc.relX,
#                                                 nav.brain.ball.loc.relY,
#                                                 nav.brain.ball.loc.bearing)
    if goToPosition.adaptive and relDest.relX >= 0:
        #reduce the speed if we're close to the target
        speed = helper.adaptSpeed(relDest.dist,
                                 constants.ADAPT_DISTANCE,
                                 goToPosition.speed)
    else:
        speed = goToPosition.speed

#    print "distance {0} and speed {1}".format(relDest.dist, speed)

    #if y-distance is small, ignore it to avoid strafing
    #strafelessDest = helper.getStrafelessDest(relDest)
    helper.setDestination(nav, relDest, speed)

#    if navTrans.shouldAvoidObstacle(nav):
#        return nav.goLater('avoidObstacle')

    return Transition.getNextState(nav, goToPosition)

def briefStand(nav):
    if nav.firstFrame():
        helper.stand(nav)

    if nav.counter > 3:
        return nav.goLater('goToPosition')

    return nav.stay()

def atPosition(nav):
    """
    Switches back if we're not at the destination anymore.
    """
    if nav.firstFrame():
        helper.stand(nav)

    return Transition.getNextState(nav, atPosition)

def walkInPlace(nav):
    if nav.firstFrame():
        helper.walkInPlace(nav)

    # if nav.counter > 20:
    #     return nav.goLater('atPosition')    

    return nav.stay()

def walking(nav):
    """
    State to be used for velocity walking.
    """
    helper.setSpeed(nav, walking.speeds)

    if navTrans.shouldDodge(nav):
        return nav.goNow('dodge')

    return Transition.getNextState(nav, walking)

def scriptedMove(nav):
    '''State that we stay in while doing sweet moves'''
    if nav.firstFrame():
        helper.executeMove(nav.brain.motion, scriptedMove.sweetMove)
        return nav.stay()

    if not nav.brain.motion.isBodyActive():
        return nav.goNow('stopped')

    return nav.stay()

def scriptedMove(nav):
    """State that we stay in while doing sweet moves"""
    if nav.firstFrame():
        helper.executeMove(nav, scriptedMove.sweetMove)
        return nav.stay()

    if not nav.brain.interface.motionStatus.body_is_active:
        return nav.goNow("stopped")

    return nav.stay()

def kickEngine(nav):
    """
    State that we stay in while calling kick engine
    """
    if nav.firstFrame():
        helper.executeKickEngine(nav, kickEngine.kickType)
        return nav.stay()

    helper.stand(nav)
    return nav.stay()

def walking(nav):
    """
    State to be used when setSpeed is called
    """

    if (walking.speeds != walking.lastSpeeds) or not nav.brain.motion.isWalkActive():
        helper.setSpeed(nav, walking.speeds)
    walking.lastSpeeds = walking.speeds

    return Transition.getNextState(nav, walking)

def walking(nav):
    """
    State to be used for velocity walking.
    """

    if (walking.speeds != walking.lastSpeeds) or not nav.brain.interface.motionStatus.walk_is_active:
        helper.setSpeed(nav, walking.speeds)
    walking.lastSpeeds = walking.speeds

    return Transition.getNextState(nav, walking)

def walking(nav):
    """
    State to be used when setSpeed is called
    """

    if ((walking.speeds != walking.lastSpeeds)
        or not nav.brain.interface.motionStatus.walk_is_active):
        helper.setSpeed(nav, walking.speeds)
    walking.lastSpeeds = walking.speeds

    return Transition.getNextState(nav, walking)