def _worldLocationUpdated(self):
robot = self._world.robot
world_pos = (robot.world_x, robot.world_y, robot.world_heading)
def get_my_dist(t):
return (hasattr(t, 'my_dist') and t.my_dist) or -1e10
bottom_top = self._world.opposing_goal.bottom_top
dists = tuple(nicefloats([get_my_dist(x), x.dist, x.focDist])
for x in bottom_top)
bearings = nicefloats([x.bearing for x in bottom_top])
if not all(isinstance(x, float) for x in world_pos):
result = "ERROR: world position not computed. It is %r. dists are %s" % (world_pos, dists)
else:
result = "position = (x = %3.3f cm | y = %3.3f cm | th = %3.3f deg), dists %s" % (
robot.world_x, robot.world_y, robot.world_heading * RAD_TO_DEG, dists)
self.log('time = %3.2f; bearing: %s, %s' % (self._world.time,
bearings, result))
self._eventmanager.quit()
def _printUpdatedCalculations(self, result):
print "inclination:", nicefloats(self._inclination)
print "joints: ", nicefloats(self._bodyAngles)
print "comheight: ", self.comHeight
print "focal WF: ", self.focalPointInWorldFrame
print "cameraToWF:\n", (self.cameraToWorldFrame * 100).astype("int")
print
v = self._v
for name, obj in [("YGLP", v.YGLP), ("YGRP", v.YGRP), ("Ball", v.Ball)]:
estimate, dist_height = self._estimates[name]
print "mine: pix=%3.3f, height=%3.3f, given: focDist=%3.3f, dist=%3.3f" % (
estimate.dist,
dist_height,
obj.focdist,
obj.distance,
)
print "mine: bearing=%3.3f, given: bearing=%3.3f" % (estimate.bearing * 180.0 / pi, obj.bearingdeg)
if self._location:
x, y, theta = self._location
theta *= 180.0 / pi
print "Location: %3.3f %3.3f %3.3f" % (x, y, theta)