def _tick(self, x, y, theta, urgency=0, keepFacing=False, relative=False, useAvoidance=True, useOnlySonarAvoid=False):
urgency = min(1.0, urgency)
self.keepFacing = keepFacing
# Convert everything to relative.
if relative:
vectorToTarget = Vector2D.Vector2D(x, y)
facingTurn = theta
else:
vectorToTarget, facingTurn = FieldGeometry.globalPoseToRobotRelativePose(Vector2D.Vector2D(x, y), theta)
facingTurn = MathUtil.normalisedTheta(facingTurn)
self.currentTarget = vectorToTarget
targetHeading = MathUtil.normalisedTheta(vectorToTarget.heading())
# forward/left are used for final adjustments, rotate to mean pos after the turn is made
if vectorToTarget.isShorterThan(400) or keepFacing:
forward = vectorToTarget.x * cos(-facingTurn) - vectorToTarget.y * sin(-facingTurn)
left = vectorToTarget.x * sin(-facingTurn) + vectorToTarget.y * cos(-facingTurn)
else:
forward = vectorToTarget.x
left = vectorToTarget.y
self.currentState.urgency = urgency
self.currentState.tick(forward, left, targetHeading, facingTurn)
def _tick(self,
x,
y,
theta,
urgency=0,
keepFacing=False,
relative=False,
useAvoidance=True,
extraObstacles=[]):
# LedOverride.override(LedOverride.leftEye, Constants.LEDColour.off)
# LedOverride.override(LedOverride.rightEye, Constants.LEDColour.off)
self.keepFacing = keepFacing
# Save destination for walkingTo
self.world.b_request.walkingToX = int(x)
self.world.b_request.walkingToY = int(y)
if relative:
new = FieldGeometry.addRrToRobot(Global.myPose(), x, y)
self.world.b_request.walkingToX = int(new[0])
self.world.b_request.walkingToY = int(new[1])
# Convert everything to relative.
if relative:
vectorToTarget = Vector2D.Vector2D(x, y)
facingTurn = theta
else:
for i in range(0, len(extraObstacles)):
extraObstacles[
i] = FieldGeometry.globalPointToRobotRelativePoint(
extraObstacles[i])
vectorToTarget, facingTurn = FieldGeometry.globalPoseToRobotRelativePose(
Vector2D.Vector2D(x, y), theta)
# always keep heading the same after avoidance
facingTurn = MathUtil.normalisedTheta(facingTurn)
targetHeading = MathUtil.normalisedTheta(vectorToTarget.heading())
if useAvoidance:
vectorToTarget = self.calculateDestinationViaObstacles(
vectorToTarget, extraObstacles)
# avoidance doesn't change which way robot's facing
self.currentTarget = vectorToTarget
forward = vectorToTarget.x
left = vectorToTarget.y
self.currentState.urgency = urgency
# myPose = Global.myPose()
# print "Pos: (%5.0f, %5.0f < %2.2f) - CurrTarget: (%5.0f, %5.0f < %2.2f) fac?%s, rel?%s, flt: (%4.0f, %4.0f < T%2.2f, F%2.2f)" % (
# myPose.x, myPose.y, degrees(myPose.theta), x, y, degrees(theta), "Y" if keepFacing else "N", "Y" if relative else "N", forward, left, degrees(targetHeading), degrees(facingTurn)
# )
self.currentState.tick(forward, left, targetHeading, facingTurn)
def _tick(self,
x,
y,
theta,
urgency=0,
keepFacing=False,
relative=False,
useAvoidance=True,
useOnlySonarAvoid=False):
urgency = min(1.0, urgency)
self.keepFacing = keepFacing
# Convert everything to relative.
if relative:
vectorToTarget = Vector2D.Vector2D(x, y)
facingTurn = theta
else:
vectorToTarget, facingTurn = FieldGeometry.globalPoseToRobotRelativePose(
Vector2D.Vector2D(x, y), theta)
facingTurn = MathUtil.normalisedTheta(facingTurn)
self.currentTarget = vectorToTarget
targetHeading = MathUtil.normalisedTheta(vectorToTarget.heading())
# forward/left are used for final adjustments, rotate to mean pos after the turn is made
if vectorToTarget.isShorterThan(400) or keepFacing:
forward = vectorToTarget.x * cos(
-facingTurn) - vectorToTarget.y * sin(-facingTurn)
left = vectorToTarget.x * sin(
-facingTurn) + vectorToTarget.y * cos(-facingTurn)
else:
forward = vectorToTarget.x
left = vectorToTarget.y
self.currentState.urgency = urgency
self.currentState.tick(forward, left, targetHeading, facingTurn)
