Python NavHelper.setSpeed方法代码示例

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import setSpeed [as 别名]
def walking(nav):
    """
    State to be used for velocity walking.
    """
    helper.setSpeed(nav, walking.speeds)

    return Transition.getNextState(nav, walking)

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

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

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

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

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

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import setSpeed [as 别名]
def avoidFrontObstacle(nav):
    # Backup
    # strafe away from the closer one?
    # strafe towards dest?

    # ever a good time to backup?
    # we'll probably want to go forward again and most obstacle
    # are moving, so pausing might make more sense

    # TODO figure this out maybe potential field will fix this for us???
    if nav.firstFrame():
        nav.doneAvoidingCounter = 0
        nav.printf(nav.brain.sonar)
        nav.printf("Avoid by backup");
        helper.setSpeed(nav, (constants.DODGE_BACK_SPEED, 0, 0))

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

    if (avoidLeft and avoidRight):
        nav.doneAvoidingCounter -= 1
        nav.doneAvoidingCounter = max(0, nav.doneAvoidingCounter)
        return nav.stay()

    elif avoidRight:
        return nav.goLater('avoidRightObstacle')

    elif avoidLeft:
        return nav.goLater('avoidLeftObstacle')

    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()

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import setSpeed [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.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.lastFast != goToPosition.fast:
    #    print "Fast changed to " + str(goToPosition.fast)

    goToPosition.lastFast = goToPosition.fast

    if goToPosition.fast:
        velX, velY, velH = 0, 0, 0

        HEADING_ADAPT_CUTOFF = 103
        DISTANCE_ADAPT_CUTOFF = 60

        MAX_TURN = 0.5

        BOOK_IT_DISTANCE_THRESHOLD = 60
        BOOK_IT_TURN_THRESHOLD = 23

        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)
            # print "velH = " + str(velH)

        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)
            # print "velX = " + str(velX)

        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, goToPosition.speed)
            # print "velY = " + str(velY)

        lastBookingIt = goToPosition.bookingIt
        if fabs(relDest.dist) > BOOK_IT_DISTANCE_THRESHOLD:
            if fabs(relDest.relH) > BOOK_IT_TURN_THRESHOLD:
                velX = 0
                velY = 0
                goToPosition.bookingIt = False
            else:
                velY = 0
                goToPosition.bookingIt = True
        else:
            goToPosition.bookingIt = False

        # if goToPosition.bookingIt != lastBookingIt:
        #    print "Booking it turned to " + str(goToPosition.bookingIt)

        goToPosition.speeds = (velX, velY, velH)

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

    else:
        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

        helper.setDestination(nav, relDest, speed)

    return Transition.getNextState(nav, goToPosition)

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

    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:
        # 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 goToPosition.fast:
        # So that fast mode works for objects of type RobotLocation also
        if isinstance(goToPosition.dest, RobotLocation) and not goToPosition.close:
            fieldDest = RobotLocation(goToPosition.dest.x, goToPosition.dest.y, 0)
            relDest = nav.brain.loc.relativeRobotLocationOf(fieldDest)
            relDest.relH = nav.brain.loc.getRelativeBearing(fieldDest)

        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,
                                    goToPosition.speed)

        if fabs(relDest.dist) > BOOK_IT_DISTANCE_THRESHOLD:
            goToPosition.close = False
            if fabs(relDest.relH) > BOOK_IT_TURN_THRESHOLD:
                if relDest.relH > 0: velH = MAX_TURN
                if relDest.relH < 0: velH = -MAX_TURN
                velX = 0
                velY = 0
                goToPosition.bookingIt = False
            else:
                velY = 0
                goToPosition.bookingIt = True
        else:
            goToPosition.close = True

        goToPosition.speeds = (velX, velY, velH)
        helper.setSpeed(nav, goToPosition.speeds)

    else:
        if goToPosition.adaptive:
            #reduce the speed if we're close to the target
            speed = helper.adaptSpeed(relDest.dist,
                                    constants.ADAPT_DISTANCE,
                                    goToPosition.speed)
        else:
            speed = goToPosition.speed

        helper.setDestination(nav, relDest, speed)

#.........这里部分代码省略.........

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import setSpeed [as 别名]

#.........这里部分代码省略.........
        # 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,
                                    goToPosition.speed)

        if fabs(relDest.dist) > BOOK_IT_DISTANCE_THRESHOLD:
            goToPosition.close = False
            if fabs(relDest.relH) > BOOK_IT_TURN_THRESHOLD:
                if relDest.relH > 0: velH = MAX_TURN
                if relDest.relH < 0: velH = -MAX_TURN
                velX = 0
                velY = 0
                goToPosition.bookingIt = False
            else:
                velY = 0
                goToPosition.bookingIt = True
        else:
            goToPosition.close = True


        # TODO nikki walk unsw hack
        # if relDest.relY < DISTANCE_ADAPT_CUTOFF:
        #     velY = 0.0

        if (fabs(relDest.relH) > 20.0):
            goToPosition.speeds = (0, 0, velH)
        else:
            goToPosition.speeds = (velX, velY, velH)
        # print("     NAV: My speeds:", velX, velY, velH)

        helper.setSpeed(nav, goToPosition.speeds)

    else:
        # print("Was not fase!")
        if goToPosition.adaptive:
            #reduce the speed if we're close to the target
            speed = helper.adaptSpeed(relDest.dist,
                                    constants.ADAPT_DISTANCE,
                                    goToPosition.speed)
        else:
            speed = goToPosition.speed

        helper.setDestination(nav, relDest, speed)
        # print("     NAV Setting dest: ", str(relDest))

    # if navTrans.shouldDodge(nav):
    #     return nav.goNow('dodge')
        
    return Transition.getNextState(nav, goToPosition)