def getGoalPostsFromVision(self):
"""Add Goalposts From Vision.
This takes vision goalposts and marks them as obstacles, so that hopefully we have one more point where we avoid
them (beyond just sonar). The main issue is that these are unfiltered.
"""
# If you see a goalpost -> Trigger a hysteresis item.
# Else decay it.
# If there is some hysteresis from a goalpost.
# If you see it: Avoid the vision goalpost.
# If you don't: Avoid the localisation one.
posts = Global.getVisionPosts()
posts_with_distance = []
obstacles = []
for post in posts:
if not 0 < post.rr.distance < (Constants.GOAL_POST_ABS_Y * 2):
# Sometimes posts are -1 in distance, I think when we don't trust our calculation. Don't try to avoid these.
# Also don't include posts we see across the field / far away, byt ignoring any over 1 goal width away.
continue
dist = max(0, post.rr.distance - Constants.GOAL_POST_DIAMETER / 2)
posts_with_distance.append(
Vector2D.makeVector2DFromDistHeading(dist, post.rr.heading))
if posts_with_distance:
self.visionPostHys.resetMax()
else:
self.visionPostHys.down()
#print "HysVal: %3d, PostsLen: %2d, DistPostsLen: %2d" % (self.visionPostHys.value, len(posts), len(posts_with_distance))
if self.visionPostHys.value > 0:
if posts_with_distance:
# If we can see the posts, avoid the posts we're seeing.
#print "Avoiding from vision"
obstacles.extend(posts_with_distance)
# print "adding posts from vision"
else:
# If we can't see a goalpost in vision, avoid the one in localisation (as an approximation so we don't
# stop avoiding as we turn our head).
robotPose = Global.myPose()
for (obs_x, obs_y, obs_diam) in WalkToPointV2.FIXED_OBSTACLES:
# Calculate distance to the obstacle.
distance, heading = MathUtil.absToRr(
(robotPose.x, robotPose.y, robotPose.theta),
(obs_x, obs_y))
# print "Adding post from localisation: (%5d < %4.0f)" % (distance, degrees(heading))
distance = max(0, distance - obs_diam / 2)
obstacles.append(
Vector2D.makeVector2DFromDistHeading(
distance, heading))
#print "Avoiding from localisation"
#return obstacles
return []
def angleToGoalShot(ballPosAbs, robotPos, robotObstacles, commitToKick):
goalRight = angleToGoalRight(ballPosAbs)
goalLeft = angleToGoalLeft(ballPosAbs)
pointsOfAim = [[angleToGoal(ballPosAbs), -1.0, Kick.MIDDLE],
[goalLeft - math.radians(10), -1.0, Kick.LEFT],
[goalRight + math.radians(10), -1.0, Kick.RIGHT]]
# If we've committed to a kick already
if Kick.MIDDLE <= commitToKick <= Kick.RIGHT:
return pointsOfAim[commitToKick]
# If goal angle is so fine that we can only kick for the middle anyway
if pointsOfAim[Kick.LEFT][0] < pointsOfAim[Kick.MIDDLE][0] or pointsOfAim[
Kick.RIGHT][0] > pointsOfAim[Kick.MIDDLE][0]:
return pointsOfAim[Kick.MIDDLE]
nearestDist = 10000
result = pointsOfAim[Kick.MIDDLE]
for i in range(0, len(robotObstacles)):
# Find position of the robots in ball relative terms
ball = (ballPosAbs.x, ballPosAbs.y, 0.0)
rr = MathUtil.absToRr(ball,
MathUtil.rrToAbs(robotPos, robotObstacles[i].rr))
dist = rr[0]
heading = rr[1]
if pointsOfAim[Kick.LEFT][0] > heading > pointsOfAim[Kick.RIGHT][0]:
if dist < nearestDist:
nearestDist = dist
if heading > pointsOfAim[Kick.MIDDLE][0]:
# Robot is to the left
#print "Aiming right"
result = pointsOfAim[Kick.RIGHT]
else:
#print "Aiming left"
# Robot is to the right
result = pointsOfAim[Kick.LEFT]
return result
def angleToGoalShot(ballPosAbs, robotPos, robotObstacles, commitToKick):
goalRight = angleToGoalRight(ballPosAbs)
goalLeft = angleToGoalLeft(ballPosAbs)
pointsOfAim = [[angleToGoal(ballPosAbs), -1.0, Kick.MIDDLE],
[goalLeft - math.radians(10), -1.0, Kick.LEFT],
[goalRight + math.radians(10), -1.0, Kick.RIGHT]]
# If we've committed to a kick already
if Kick.MIDDLE <= commitToKick <= Kick.RIGHT:
return pointsOfAim[commitToKick]
# If goal angle is so fine that we can only kick for the middle anyway
if pointsOfAim[Kick.LEFT][0] < pointsOfAim[Kick.MIDDLE][0] or pointsOfAim[Kick.RIGHT][0] > pointsOfAim[Kick.MIDDLE][0]:
return pointsOfAim[Kick.MIDDLE]
nearestDist = 10000
result = pointsOfAim[Kick.MIDDLE]
for i in range(0,len(robotObstacles)):
# Find position of the robots in ball relative terms
ball = (ballPosAbs.x, ballPosAbs.y, 0.0)
rr = MathUtil.absToRr(ball, MathUtil.rrToAbs(robotPos, robotObstacles[i].rr))
dist = rr[0]
heading = rr[1]
if pointsOfAim[Kick.LEFT][0] > heading > pointsOfAim[Kick.RIGHT][0]:
if dist < nearestDist:
nearestDist = dist
if heading > pointsOfAim[Kick.MIDDLE][0]:
# Robot is to the left
#print "Aiming right"
result = pointsOfAim[Kick.RIGHT]
else:
#print "Aiming left"
# Robot is to the right
result = pointsOfAim[Kick.LEFT]
return result
