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