Python NavHelper.stand方法代码示例

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def stand(nav):
    """
    Transitional state between walking and standing
    So we can give new walk commands before we complete
    the stand if desired
    """
    if (nav.brain.player.gameState == 'gameInitial'
     or nav.brain.player.gameState == 'gameSet'
     or nav.lastState == 'stand'):
        helper.stand(nav)
        return nav.stay()

    if nav.counter < 10:
        # print("In stand, walking in place")
        helper.walkInPlace(nav)
        return nav.stay()

    elif nav.counter < 15:
        helper.stand(nav)
    # if nav.firstFrame():
    #     helper.stand(nav)
    #     return nav.stay()

    # if (nav.counter % 300 == 0):
    #     return nav.stay()

    if not nav.brain.interface.motionStatus.walk_is_active:
        return nav.goNow('standing')

    # helper.stand(nav)

    return nav.stay()

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def destinationWalkingTo(nav):
    """
    State to be used for destination walking.
    """
    if nav.firstFrame():
        destinationWalkingTo.enqueAZeroVector = False

    if not nav.brain.motion.calibrated:
        helper.stand(nav)
        return nav.stay()

    if nav.counter < 4:
        # print("In dest walking to, walking in place")
        helper.walkInPlace(nav)
        return nav.stay()

    destinationWalkingTo.speed = nav.velocity
    if fabs(nav.requestVelocity - nav.velocity) > Navigator.SPEED_CHANGE:
            nav.velocity += copysign(Navigator.SPEED_CHANGE, (nav.requestVelocity - nav.velocity))

    if len(destinationWalkingTo.destQueue) > 0:
        dest = destinationWalkingTo.destQueue.popleft()
        helper.setDestination(nav, dest,
                              destinationWalkingTo.speed,
                              destinationWalkingTo.kick)
        # destinationWalkingTo.enqueAZeroVector = True
    elif destinationWalkingTo.enqueAZeroVector:
        helper.setDestination(nav, RelRobotLocation(0,0,0),
                              destinationWalkingTo.speed,
                              destinationWalkingTo.kick)
        destinationWalkingTo.enqueAZeroVector = False

    return nav.stay()

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def briefStand(nav):
    if nav.firstFrame():
        helper.stand(nav)

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

    return nav.stay()

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def atPosition(nav):
    """
    Switches back if we're not at the destination anymore.
    """
    if nav.firstFrame():
        helper.stand(nav)

    return Transition.getNextState(nav, atPosition)

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
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()

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def atPosition(nav):
    """
    Switches back if we're not at the destination anymore.
    """
    if nav.firstFrame():
        # print("In at position, walking in place")
        helper.walkInPlace(nav)
    elif nav.counter == 5:
        helper.stand(nav)

    return Transition.getNextState(nav, atPosition)

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def stop(nav):
    """
    Wait until the walk is finished.
    """
    if nav.firstFrame():
        # stop walk vectors
        helper.stand(nav)
        
    if not nav.brain.motion.isWalkActive():
        return nav.goNow('stopped')

    return nav.stay()

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def stand(nav):
    """
    Transitional state between walking and standing
    Could still be walking, but we can give it new walk commands
    so we shouldn't wait to go for it to go to standing before we
    give it new commands
    """
    if nav.firstFrame():
        helper.stand(nav)

    if not nav.brain.motion.isWalkActive():
        return nav.goNow('standing')

    return nav.stay()

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def walkingTo(nav):
    """
    Walks to a relative location based on odometry
    """
    if nav.firstFrame():
        helper.stand(nav)
        return nav.stay()

    if nav.brain.motion.isStanding():
        if len(walkingTo.destQueue) > 0:
            dest = walkingTo.destQueue.popleft()
            helper.setOdometryDestination(nav, dest, walkingTo.speed)
            return nav.stay()
        else:
            return nav.goNow('standing')

    return nav.stay()

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def walkingTo(nav):
    """
    State to be used for odometry walking.
    """
    if nav.firstFrame():
        helper.stand(nav)
        return nav.stay()

    if nav.brain.interface.motionStatus.standing:
        if len(walkingTo.destQueue) > 0:
            dest = walkingTo.destQueue.popleft()
            helper.setOdometryDestination(nav, dest, walkingTo.speed)
            return nav.stay()
        else:
            return nav.goNow("standing")

    return nav.stay()

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def walkingTo(nav):
    """
    State to be used for odometry walking.
    """
    if nav.firstFrame():
        print ("Resetting odometry!")
        nav.brain.interface.motionRequest.reset_odometry = True
        nav.brain.interface.motionRequest.timestamp = int(nav.brain.time * 1000)
        print ("MY dest: ", nav.destination.relX, nav.destination.relY, nav.destination.relH)
        helper.walkInPlace(nav)
        return nav.stay()

    
    if not nav.brain.motion.calibrated:
        helper.stand(nav)
        return nav.stay()

    if nav.counter < 5:
        return nav.stay()

    walkingTo.currentOdo = RelRobotLocation(nav.brain.interface.odometry.x,
                         nav.brain.interface.odometry.y,
                         nav.brain.interface.odometry.h)

    # TODO why check standing?
    if nav.brain.interface.motionStatus.standing:

        if len(walkingTo.destQueue) > 0:
            dest = walkingTo.destQueue.popleft()
            # dest.relH = 0
            helper.setOdometryDestination(nav, dest, walkingTo.speed)
            # helper.setDestination(nav, dest, walkingTo.speed)
            print ("MY dest: ", dest.relX, dest.relY, dest.relH)

    if locationsMatch(nav.destination, walkingTo.currentOdo):
        print("I think i'm there!")
        return nav.goNow('standing')

    # if nav.counter % 10 == 0:
    #     print "Current odo:"
    #     print ("x:", walkingTo.currentOdo.relX)
    #     print ("y:", walkingTo.currentOdo.relY)
    #     print ("h:", walkingTo.currentOdo.relH)

    return nav.stay()

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def stand(nav):
    """
    Transitional state between walking and standing
    So we can give new walk commands before we complete
    the stand if desired
    """
    if nav.firstFrame():
        helper.stand(nav)
        return nav.stay()

    if nav.counter % 300 == 0:
        helper.stand(nav)
        return nav.stay()

    if not nav.brain.interface.motionStatus.walk_is_active:
        return nav.goNow("standing")

    return nav.stay()

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def walkingTo(nav):
    """
    State to be used for odometry walking.
    """
    if nav.firstFrame():
        print ("Resetting odometry!")
        nav.brain.interface.motionRequest.reset_odometry = True
        nav.brain.interface.motionRequest.timestamp = int(nav.brain.time * 1000)
        helper.stand(nav)
        return nav.stay()


    walkingTo.currentOdo = RelRobotLocation(nav.brain.interface.odometry.x,
                         nav.brain.interface.odometry.y,
                         nav.brain.interface.odometry.h)

    # TODO why check standing?
    if nav.brain.interface.motionStatus.standing:

        if len(walkingTo.destQueue) > 0:
            dest = walkingTo.destQueue.popleft()
            helper.setOdometryDestination(nav, dest, walkingTo.speed)
            print ("MY dest: ", dest.relX, dest.relY, dest.relH)

    if locationsMatch(nav.destination, walkingTo.currentOdo):
        return nav.goNow('standing')

    # walkingTo.currentOdow = RelRobotLocation(nav.brain.interface.odometry.x,
    #                          nav.brain.interface.odometry.y,
    #                          nav.brain.interface.odometry.h)
    if nav.counter % 30 == 0:
        print "Current odo:"
        print ("x:", walkingTo.currentOdo.relX)
        print ("y:", walkingTo.currentOdo.relY)
        print ("h:", walkingTo.currentOdo.relH)
        # print "Current odow:"
        # print ("x:", walkingTo.currentOdow.relX)
        # print ("y:", walkingTo.currentOdow.relY)
        # print ("h:", walkingTo.currentOdow.relH)
        # print "---------------"



    return nav.stay()

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
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.loc, goToPosition.dest)

    if nav.firstFrame():
        # print("Resetting at position transition!!")
        nav.atLocPositionTransition.reset()

    if not nav.brain.motion.calibrated:
        helper.stand(nav)
        return nav.stay()

    # if nav.counter % 10 is 0:
    # print "\ngoing 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 nav.counter < 5:
        # print("In go to position, walking in place")
        helper.walkInPlace(nav)
        return nav.stay()

    goToPosition.speed = nav.velocity
    if fabs(nav.requestVelocity - nav.velocity) > Navigator.SPEED_CHANGE:
        nav.velocity += copysign(Navigator.SPEED_CHANGE, (nav.requestVelocity - nav.velocity))

    if goToPosition.pb and isinstance(goToPosition.dest, RelRobotLocation):
        # Calc dist to dest
        dist = helper.getDistToDest(nav.brain.loc, goToPosition.dest)
        if goToPosition.fast and dist < 140:
            goToPosition.fast = False
            goToPosition.dest = nav.brain.play.getPosition()
        elif not goToPosition.fast and dist > 160:
            goToPosition.fast = True
            goToPosition.dest = nav.brain.play.getPositionCoord()

    if isinstance(goToPosition.dest, RobotLocation):
        dist = helper.getDistToDest(nav.brain.loc, goToPosition.dest)
        # print("Distance: ", dist)
        if dist < 30:
            # print("I'm close enough ! I should not go fast anymore")
            goToPosition.fast = False
            goToPosition.speeds = (0.1, 0.1, 0.1)

    # print("My reldest: ", str(relDest))

    if goToPosition.fast:
        # print("goToPosition fast")
        # So that fast mode works for objects of type RobotLocation also
        if isinstance(goToPosition.dest, RobotLocation) and not goToPosition.close:
            # print("It is an instance of a robot location")
            fieldDest = RobotLocation(goToPosition.dest.x, goToPosition.dest.y, 0)
            relDest = nav.brain.loc.relativeRobotLocationOf(fieldDest)
            relDest.relH = nav.brain.loc.getRelativeBearing(fieldDest)
        elif isinstance(goToPosition.dest, RelRobotLocation):
            relDest = goToPosition.dest



        HEADING_ADAPT_CUTOFF = 103
        DISTANCE_ADAPT_CUTOFF = 10

        MAX_TURN = .5

        BOOK_IT_TURN_THRESHOLD = 23
        BOOK_IT_DISTANCE_THRESHOLD = 50

        if relDest.relH >= HEADING_ADAPT_CUTOFF:
            velH = MAX_TURN
        elif relDest.relH <= -HEADING_ADAPT_CUTOFF:
            velH = -MAX_TURN
        else:
            velH = helper.adaptSpeed(relDest.relH,
                                    HEADING_ADAPT_CUTOFF,
                                    MAX_TURN)

        if relDest.relX >= DISTANCE_ADAPT_CUTOFF:
            velX = goToPosition.speed
        elif relDest.relX <= -DISTANCE_ADAPT_CUTOFF:
            velX = -goToPosition.speed
        else:
            velX = helper.adaptSpeed(relDest.relX,
                                    DISTANCE_ADAPT_CUTOFF,
                                    goToPosition.speed)

        if relDest.relY >= DISTANCE_ADAPT_CUTOFF:
            velY = goToPosition.speed
        elif relDest.relY <= -DISTANCE_ADAPT_CUTOFF:
            velY = -goToPosition.speed
        else:
            velY = helper.adaptSpeed(relDest.relY,
                                    DISTANCE_ADAPT_CUTOFF,
#.........这里部分代码省略.........

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import stand [as 别名]
def standing(nav):
    if nav.firstFrame():
        helper.stand(nav)
        
    return nav.stay()