def strafe(self):
print "strafing"
if self.goalLocationKnown:
if self.goalpost_to_track.bearing < -DEFAULT_NORMALIZED_CENTERING_Y_ERROR:
if burst.connecting_to_webots():
self.movement_deferred = self._actions.turn(-0.2)
self.movement_deferred.onDone(self.strafe)
else:
self.movement_deferred = self._actions.executeTurnCW()
self.movement_deferred.onDone(self.strafe)
elif self.goalpost_to_track.bearing > DEFAULT_NORMALIZED_CENTERING_Y_ERROR:
if burst.connecting_to_webots():
self.movement_deferred = self._actions.turn(0.2)
self.movement_deferred.onDone(self.strafe)
else:
self.movement_deferred = self._actions.executeTurnCCW()
self.movement_deferred.onDone(self.strafe)
else:
print "Aligned position reached!"
self._actions.tracker.stop()
#self.ballLocationKnown = False
#self._actions.executeHeadMove(moves.HEAD_MOVE_FRONT_BOTTOM).onDone(self.doNextAction)
self._eventmanager.quit()
else:
print "Goalpost lost, restart search"
self.searchGoalPosts()
def _finishInit(self, result):
""" called after we have a camera registration and can start receiving
images. Does all initialization that can be postponed (read the imops
library, read the color tables), and actually starts the task to retrieve
images (which consumes loads of bandwidth) """
#actually, let's set the correct table here:
if burst.connecting_to_webots():
self.webots_table()
else:
self.default_table()
self.imops.update_table()
self._startTaskFirstTime()
def __init__(self, world):
self.cameraToWorldFrame = identity()
self.focalPointInWorldFrame = identity()
self.comHeight = 0 # MM cause that's the way they do it.
self.cameraToHorizonFrame = identity()
self.horizonSlope = 0.0
self.horizonLeft = [0.0, 0.0]
self.horizonRight = [0.0, 0.0]
self._inclination_vars = [
"Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value",
"Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value",
]
self._inclination = [0.0, 0.0]
self.updateTransforms([0.0] * 26, [0.0, 0.0]) # init stuff
self._connecting_to_webots = burst.connecting_to_webots()
from burst_consts import IMAGE_HALF_WIDTH, IMAGE_HALF_HEIGHT
from burst_util import normalize2
if burst.running_on_nao():
base='/opt/naoqi/'
else:
base=os.environ['AL_DIR']
TABLE = os.path.join(base, 'modules/etc/table.mtb')
WHICH = os.path.join(base, 'modules/etc/whichtable.txt')
which = 'undefined'
if os.path.exists(WHICH):
with open(WHICH) as fd:
which=fd.read().strip()
if (('webots' in which and burst.connecting_to_nao()) or
('webots' not in which and burst.connecting_to_webots())):
print "WARNING: you are using the incorrect color table:"
print "which = %s" % which
def get_nao_mtb():
if not os.path.exists(TABLE):
print "WARNING: get_nao_mtb can't find the table %s" % TABLE
return None
with open(TABLE) as fd:
table = fd.read()
return table
def return_index_to_rgb():
""" For use with imops.write_index_to_rgb
for usage with the debug imops.thresholded_to_rgb
"""