Python NavHelper.setDestination方法代码示例

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

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

    # TODO: HACK FOR BRAZIL - THIS IS USED WHEN IT'S ARM-ONLY DETECTION
    if nav.firstFrame():
        ## 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
        if dodge.armPosition == 1:
            print "Dodging NORTH obstacle"
            relDest = helper.getRelativeDestination(nav.brain.loc,
                                                    goToPosition.dest)
            if relDest.relY <= 0:
                direction = -1
            else:
                direction = 1
            dodge.dest = RelRobotLocation(-15, direction*10, 0)
        elif dodge.armPosition == 2:
            print "Dodging NORTHEAST obstacle"
            dodge.dest = RelRobotLocation(-5, 15, 0)
        elif dodge.armPosition == 3:
            print "Dodging EAST obstacle"
            dodge.dest = RelRobotLocation(0, 20, 0)
        elif dodge.armPosition == 4:
            print "Dodging SOUTHEAST obstacle"
            dodge.dest = RelRobotLocation(5, 15, 0)
        # if directly behind us, move forward and to one side based on
        # where the goToPosition dest is
        elif dodge.armPosition == 5:
            print "Dodging SOUTH obstacle"
            relDest = helper.getRelativeDestination(nav.brain.loc,
                                                    goToPosition.dest)
            if relDest.relY <= 0:
                direction = -1
            else:
                direction = 1
            dodge.dest = RelRobotLocation(15, direction*10, 0)
        elif dodge.armPosition == 6:
            print "Dodging SOUTHWEST obstacle"
            dodge.dest = RelRobotLocation(5, -15, 0)
        elif dodge.armPosition == 7:
            print "Dodging WEST obstacle"
            dodge.dest = RelRobotLocation(0, -20, 0)
        elif dodge.armPosition == 8:
            print "Dodging NORTHWEST obstacle"
            dodge.dest = RelRobotLocation(-5, -15, 0)
        else:
            return

    dest = RelRobotLocation(dodge.dest.relX + random(),
                            dodge.dest.relY + random(),
                            dodge.dest.relH + random())
    helper.setDestination(nav, dest, 0.5)
    return Transition.getNextState(nav, dodge)

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

    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 setDestination [as 别名]
def dodge(nav):
    order = [0, 1, -1, 2, -2, 3, -3, 4]
    if nav.firstFrame():
        # dodge.positions[0] is position.NONE, so direction numbers are their own index
        for i in range(len(order)):
            temp = getIndex(int(dodge.targetDest) + order[i])
            # if there is no obstacle in this direction
            if not dodge.positions[temp]:
                print "DODGE TO ", dodge.DDirects[temp]
                dodge.dest = RelRobotLocation(constants.DGE_DESTS[temp-1][0],
                                              constants.DGE_DESTS[temp-1][1],
                                              constants.DGE_DESTS[temp-1][2])
                break

    # TODO the worst hack I have ever written, sorry -- Josh Imhoff
    dest2 = RelRobotLocation(dodge.dest.relX + random(),
                             dodge.dest.relY + random(),
                             dodge.dest.relH + random())
    helper.setDestination(nav, dest2, 0.5)
    return Transition.getNextState(nav, dodge)

# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import setDestination [as 别名]
def walkingTo(nav):
    """
    Walks to a relative location based on odometry
    """
    loc = nav.brain.loc
    dest = walkingTo.dest
        
    if nav.firstFrame():
        #@todo: make a method that returns odometry as a tuple in PyLoc?
        walkingTo.startingOdometry = (loc.lastOdoX, loc.lastOdoY, loc.lastOdoTheta)
    
    deltaOdo = helper.getDeltaOdometry(loc, walkingTo.startingOdometry)
    walkingTo.deltaDest = dest - (deltaOdo.relX, deltaOdo.relY, deltaOdo.relH)
#    print "Delta dest {0}".format(walkingTo.deltaDest)
#    print str(dest)
#    print str(deltaOdo)
    #walk the rest of the way
    helper.setDestination(nav, walkingTo.deltaDest, walkingTo.speed)
    
    return Transition.getNextState(nav, walkingTo)

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

    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 setDestination [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 setDestination [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 setDestination [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)