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 getSonarObstacles():
obstacles = []
LEFT, RIGHT = 0, 1
nearbyObject = [
Sonar.hasNearbySonarObject(LEFT),
Sonar.hasNearbySonarObject(RIGHT)
]
sonarDebug = ""
if nearbyObject[LEFT]:
sonarDebug += " left"
obstacles.append(
Vector2D.makeVector2DFromDistHeading(Constants.ROBOT_DIAM,
radians(30)))
if nearbyObject[RIGHT]:
sonarDebug += " right"
obstacles.append(
Vector2D.makeVector2DFromDistHeading(Constants.ROBOT_DIAM,
-radians(30)))
if sonarDebug:
robot.say("sonar" + sonarDebug)
return obstacles
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
