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)
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 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 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)
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)
if navTrans.shouldDodge(nav):
return nav.goNow('dodge')
return Transition.getNextState(nav, goToPosition)
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,
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)
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)
return Transition.getNextState(nav, goToPosition)
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:
# 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)
return Transition.getNextState(nav, goToPosition)
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,
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)
