本文整理汇总了Python中NavHelper.getDistToDest方法的典型用法代码示例。如果您正苦于以下问题：Python NavHelper.getDistToDest方法的具体用法？Python NavHelper.getDistToDest怎么用？Python NavHelper.getDistToDest使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类NavHelper的用法示例。
在下文中一共展示了NavHelper.getDistToDest方法的3个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: goToPosition
# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import getDistToDest [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.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:
velX, velY, velH = 0, 0, 0
HEADING_ADAPT_CUTOFF = 103
DISTANCE_ADAPT_CUTOFF = 60
MAX_TURN = .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:
#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)
#.........这里部分代码省略.........
示例2: goToPosition
# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import getDistToDest [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)
#.........这里部分代码省略.........
示例3: goToPosition
# 需要导入模块: import NavHelper [as 别名]
# 或者: from NavHelper import getDistToDest [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,
#.........这里部分代码省略.........