def _updateXYTheta_from_twoDistOneAngle(self, top=None, bottom=None, debug=False):
""" Take two objects and update our self location in WF from it.
NOTE: the objects need to be ordered - top_y > bottom_y
Defaults to Yellow Goal Left Post and Right Post (Top and Bottom
respectively)
"""
from burst.position import xyt_from_two_dist_one_angle
if top is not None or bottom is not None:
raise NotIMplementedError("Not done yet")
opposing_lp, opposing_rp = self._v.YGLP, self._v.YGRP
if opposing_lp.distance != 0.0 and opposing_rp.distance != 0.0:
(e_left, r1), (e_right, r2) = (self._estimates["YGLP"], self._estimates["YGRP"])
p0 = field.yellow_goal.top_post.xy
p1 = field.yellow_goal.bottom_post.xy
d = field.CROSSBAR_CM_WIDTH / 2.0
# XXX: I think the YGLP is the BOTTOM, not the TOP
if e_right.dist == 0.0:
# print "using given bearing"
a1 = opposing_rp.bearingdeg * pi / 180.0
else:
a1 = e_right.bearing
x, y, theta = xyt_from_two_dist_one_angle(r1=r1, r2=r2, a1=a1, d=d, p0=p0, p1=p1, debug=debug)
self._location = (x, y, theta)
self._location_origin = (r1, r2, a1, d, p0, p1)
return x, y, theta
return None
def updateRobotPosition(self):
""" tries to update robot position from current landmarks. return value is success """
d = CROSSBAR_CM_WIDTH / 2.0
bottom, top = self._bottom, self._top
p0 = top.xy
p1 = bottom.xy
# TODO - use all dists, compare
if not hasattr(top, 'my_dist') or not hasattr(bottom, 'my_dist'):
if self.verbose:
print "BUG ALERT: we should have my_dist updated at this point."
return False
r1, r2, a1 = (top.my_dist, bottom.my_dist, top.bearing)
# compute distance using r_avg and angle - note that it is not correct
if abs(r1 - r2) > 2*d:
if self.verbose:
print "Localization: Warning: inputs are bad, need to recalculate"
# This is fun: which value do I throw away? I could start
# collecting a bunch first, and only if it is well localized (looks
# like a nice normal distribution) I use it..
return False
x, y, theta = xyt_from_two_dist_one_angle(
r1=r1, r2=r2, a1=a1, d=d, p0=p0, p1=p1)
min_x, max_x, min_y, max_y = white_limits # This is the WHITE limits - we could be in the green part.. COMPETITION
if not (min_x <= x <= max_x) or not (min_y <= y <= max_y):
#import pdb; pdb.set_trace()
print "LOCALIZATION ERROR - out of White bounds; not updating"
print "Error value: %3.2f %3.2f %3.2f deg" % (x, y, theta*RAD_TO_DEG)
return False
if self.verbose:
print "Localization: GOT %3.2f %3.2f heading %3.2f deg" % (x, y, theta*RAD_TO_DEG)
r = self._world.robot
r.world_x, r.world_y, r.world_heading = x, y, theta
r.world_position_update_time = self._world.time
self._history.append((r.world_position_update_time, r.world_x, r.world_y, r.world_heading))
return True
def calc_goal_coord(self, sglp, sgrp, uglp, ugrp):
"""
Update position of unseen goal.
"""
if HALF_GOAL_SIZE>=sglp.dist or HALF_GOAL_SIZE>=sgrp.dist:
return
#debug
#(x1,y1, theta1) = xyt_from_two_dist_one_angle(200, 250, 0,HALF_GOAL_SIZE , (0, HALF_GOAL_SIZE) ,(0, -HALF_GOAL_SIZE) )
(x1,y1, theta1) = xyt_from_two_dist_one_angle(sglp.dist, sgrp.dist, sglp.bearing,HALF_GOAL_SIZE , (0, HALF_GOAL_SIZE) ,(0, -HALF_GOAL_SIZE) )
if x1 and y1:
uglp.dist = ((FIELD_SIZE + x1)**2 + (-HALF_GOAL_SIZE + y1)**2)**0.5
ugrp.dist = ((FIELD_SIZE + x1)**2 + (HALF_GOAL_SIZE + y1)**2)**0.5
uglp.bearing = asin(abs(-HALF_GOAL_SIZE + y1) / uglp.dist)
ugrp.bearing = asin(abs(HALF_GOAL_SIZE + y1) / ugrp.dist)
else:
#print "NOTICE: localization->calc_goal_coord: x1/y1/theta1 is None"
pass
