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 init(self, keepFacing=False, relative=False, useAvoidance=True):
self.keepFacing = keepFacing
self.relativeTarget = relative
self.currentState = Initial(self)
self.currentTarget = Vector2D.Vector2D(0, 0)
self.target = None
self.movingAvoidanceHistory = []
self.avoidanceHys = Hysteresis(-10, 10)
self.visionPostHys = Hysteresis(0, 10)
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 getSharedKickoffTarget():
side_of_field = 1
upfielder = getFirstOfRole(Constants.ROLE_UPFIELDER)
if upfielder >= 0:
upfielder_pos, _ = getTeammatePose(upfielder)
side_of_field = math.copysign(1, upfielder_pos.y)
else:
midfielder = getFirstOfRole(Constants.ROLE_MIDFIELDER)
if midfielder >= 0:
midfielder_pos, _ = getTeammatePose(midfielder)
side_of_field = math.copysign(1, midfielder_pos.y)
# Now we have a side of field to kick to.
return Vector2D.Vector2D(2500, 1200 * side_of_field)
def get_bot_position(bot, traceable_object, tracker, samples=3, debug=False):
xSamples = []
ySamples = []
for sample in range(samples):
image = tracker.get_video_frame()
timestamp = time.time()
if sample > 0: # ignore the first sample
x, y = tracker._find_traceable_in_image(
image, traceable_object) # side effect: adds mask to tracker
if x:
xSamples.append(x)
if y:
ySamples.append(y)
traceable_object.add_tracking(Vector2D(x, y), timestamp)
if debug:
display_current_view(tracker)
print "{} | {} ".format(xSamples, ySamples)
return (sum(xSamples) / len(xSamples)), (sum(ySamples) / len(ySamples))
def calculateDestinationViaObstacles(self, vectorToTarget, extraObstacles):
isSupporter = len(extraObstacles) > 0
"attractive field of target"
Uatt = PotentialField.getAttractiveField(vectorToTarget)
# let x axis be path
targetHeading = vectorToTarget.heading()
rotatedVectorToTarget = vectorToTarget.rotated(-targetHeading)
# dont avoid obstacles within 100mm to target
rotatedVectorToTarget.x -= copysign(100, rotatedVectorToTarget.x)
"repulsive field of each obstacle"
Urep = Vector2D.Vector2D(0, 0)
obstacles = Global.robotObstaclesList()
for i in range(0, len(obstacles)):
# LedOverride.override(LedOverride.leftEye, Constants.LEDColour.magenta)
logObstacle(obstacles[i].rr.distance, obstacles[i].rr.heading)
obsPos = Vector2D.makeVector2DFromDistHeading(
obstacles[i].rr.distance, obstacles[i].rr.heading)
if isSupporter or isNearPathToTarget(
targetHeading, rotatedVectorToTarget, obsPos):
Urep = Urep.plus(PotentialField.getRepulsiveField(obsPos))
# add sonar and goal posts
obstacles = self.getSonarObstacles()
obstacles.extend(self.getGoalPostsFromVision())
obstacles.extend(extraObstacles)
for i in range(0, len(obstacles)):
logObstacle(obstacles[i].length(), obstacles[i].heading())
if isSupporter or isNearPathToTarget(
targetHeading, rotatedVectorToTarget, obstacles[i]):
Urep = Urep.plus(PotentialField.getRepulsiveField(
obstacles[i]))
# add closest points to field borders
myPos = Global.myPos()
myHeading = Global.myHeading()
BORDER_PADDING = 800
obstacles = [
Vector2D.Vector2D(
Constants.FIELD_LENGTH / 2 + BORDER_PADDING - myPos.x, 0),
Vector2D.Vector2D(
-(Constants.FIELD_LENGTH / 2 + BORDER_PADDING) - myPos.x, 0),
Vector2D.Vector2D(
0, Constants.FIELD_WIDTH / 2 + BORDER_PADDING - myPos.y),
Vector2D.Vector2D(
0, -(Constants.FIELD_WIDTH / 2 + BORDER_PADDING) - myPos.y)
]
for i in range(0, len(obstacles)):
obstacle = obstacles[i].rotate(-myHeading)
logObstacle(obstacle.length(), obstacle.heading())
if isSupporter or isNearPathToTarget(
targetHeading, rotatedVectorToTarget, obstacle):
Urep = Urep.plus(PotentialField.getRepulsiveField(obstacle))
#sum the fields, we only need the heading change
U = Uatt.plus(Urep)
# avoid losing too much forward momentum
headingDiff = clamp(U.heading() - vectorToTarget.heading(),
radians(-70), radians(70))
vectorToTarget.rotate(headingDiff)
logTarget(vectorToTarget, headingDiff)
return vectorToTarget
def angleIntoField():
toCentre = globalPointToRobotRelativePoint(Vector2D.Vector2D(0, 0))
return toCentre.heading()
def angleBetweenBallAndGoalCenter():
ballPos = blackboard.localisation.ballPos
ballVec = Vector2D.Vector2D(ballPos.x, ballPos.y)
goalCenter = OWN_GOAL_CENTER
phi = Vector2D.angleBetween(ballVec, goalCenter)
return MathUtil.normalisedTheta(phi)
import math
import Global
import Constants
blackboard = None
class Kick(object):
NONE = -1
MIDDLE = 0
LEFT = 1
RIGHT = 2
# Enemy goal vectors.
ENEMY_GOAL_CENTER = Vector2D.Vector2D(Constants.FIELD_LENGTH / 2.0, 0)
ENEMY_GOAL_BEHIND_CENTER = Vector2D.Vector2D(
Constants.FIELD_LENGTH / 2.0 + 100,
0) # +100 offset so angles aren't too sharp near goals
ENEMY_GOAL_INNER_LEFT = Vector2D.Vector2D(
Constants.FIELD_LENGTH / 2.0,
Constants.GOAL_POST_ABS_Y - (Constants.GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_INNER_RIGHT = Vector2D.Vector2D(
Constants.FIELD_LENGTH / 2.0,
-Constants.GOAL_POST_ABS_Y + (Constants.GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_OUTER_LEFT = Vector2D.Vector2D(
Constants.FIELD_LENGTH / 2.0,
Constants.GOAL_POST_ABS_Y + (Constants.GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_OUTER_RIGHT = Vector2D.Vector2D(
Constants.FIELD_LENGTH / 2.0,
-Constants.GOAL_POST_ABS_Y - (Constants.GOAL_POST_DIAMETER / 2))
from util import TeamStatus
import math
import Global
from Constants import (
FIELD_LENGTH,
FIELD_WIDTH,
GOAL_POST_ABS_Y,
GOAL_POST_DIAMETER,
MARKER_CENTER_X,
)
blackboard = None
# Enemy goal vectors.
OFFSET_REDUCE_SHARPNESS = 150 # so angles aren't too sharp near goals
ENEMY_GOAL_CENTER = Vector2D.Vector2D(FIELD_LENGTH / 2.0, 0)
ENEMY_GOAL_BEHIND_CENTER = Vector2D.Vector2D(
FIELD_LENGTH / 2.0 + OFFSET_REDUCE_SHARPNESS, 0)
ENEMY_GOAL_INNER_LEFT = Vector2D.Vector2D(
FIELD_LENGTH / 2.0, GOAL_POST_ABS_Y - (GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_INNER_RIGHT = Vector2D.Vector2D(
FIELD_LENGTH / 2.0, -GOAL_POST_ABS_Y + (GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_OUTER_LEFT = Vector2D.Vector2D(
FIELD_LENGTH / 2.0, GOAL_POST_ABS_Y + (GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_OUTER_RIGHT = Vector2D.Vector2D(
FIELD_LENGTH / 2.0, -GOAL_POST_ABS_Y - (GOAL_POST_DIAMETER / 2))
ENEMY_GOAL_CORNER_LEFT = Vector2D.Vector2D(FIELD_LENGTH / 2.0, FIELD_WIDTH / 2)
ENEMY_GOAL_CORNER_RIGHT = Vector2D.Vector2D(FIELD_LENGTH / 2.0,
-FIELD_WIDTH / 2)
ENEMY_PENALTY_CENTER = Vector2D.Vector2D(MARKER_CENTER_X, 0)
def getTeammatePose(index):
pose = blackboard.receiver.data[index].robotPos
return (Vector2D.Vector2D(pose.x, pose.y), pose.theta)
