def __init__(self):
if burst.options.trace_proxies:
print "INFO: Proxy tracing is on"
# avoid logging None objects - like when getSpeechProxy returns None
callWrapper = lambda name, obj: (obj and LogCalls(name, obj) or obj)
else:
callWrapper = lambda name, obj: obj
self._memory = callWrapper("ALMemory", burst.getMemoryProxy(deferred=True))
self._motion = callWrapper("ALMotion", burst.getMotionProxy(deferred=True))
self._sentinel = callWrapper("ALSentinel", burst.getSentinelProxy(deferred=True))
self._speech = callWrapper("ALSpeech", burst.getSpeechProxy(deferred=True))
self._video = callWrapper("ALVideoDevice", burst.getALVideoDeviceProxy(deferred=True))
self._leds = callWrapper("ALLeds", burst.getLedsProxy(deferred=True))
self._imops = callWrapper("imops", burst.getImopsProxy(deferred=True))
if burst.options.run_sonar:
self._sonar = callWrapper("ALSonar", burst.getSonarProxy(deferred=True))
self._events = set()
self._deferreds = []
self.burst_deferred_maker = BurstDeferredMaker()
# This makes sure stuff actually works if nothing is being updated on the nao.
self._default_proxied_variable_value = 0.0
# We do memory.getListData once per self.update, in one go.
# later when this is done with shared memory, it will be changed here.
# initialize these before initializing objects that use them (Ball etc.)
default_vars = self.getDefaultVars()
self._vars_to_get_set = set()
self._vars_to_get_list = list()
self.vars = {} # no leading underscore - meant as a public interface (just don't write here, it will be overwritten every update)
self.addMemoryVars(default_vars)
self._shm = None
self.time = 0.0
self.start_time = time() # construction time
# Variables for body joint angles from dcm
self._getAnglesMap = dict([(joint,
'Device/SubDeviceList/%s/Position/Sensor/Value' % joint)
for joint in self.jointnames])
if self.connected_to_webots and is_120:
print "World: Using ALMotion for body angles"
self._updateBodyAngles = self._updateBodyAngles_from_ALMotion
else:
print "World: Using DCM for body angles"
self._body_angles_vars = self._getAnglesMap.values()
self._updateBodyAngles = self._updateBodyAngles_from_DCM
self.addMemoryVars(self._body_angles_vars)
self.body_angles = [0.0] * 26
# Variables for Inclination angles
self._inclination_vars = ['Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value',
'Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value']
self.addMemoryVars(self._inclination_vars)
self._recorded_vars = self._getRecorderVariableNames()
print "world will record (if asked) %s vars" % len(self._recorded_vars)
self._recorded_header = self._recorded_vars
self._record_basename = World.running_on_nao and '/media/userdata' or '/tmp'
# Stuff that we prefer the users use directly doesn't get a leading
# underscore
# * Vision recognized objects
self.ball = Ball(self)
# Goals Notes: We start at UNCONFIGURED State, we record all the variables
# but only start using them once we configure all the goal according to our team and
# goal color.
our_lp_xy = field.our_goal.top_post.xy # left is from pov of goalie looking at opponent goal.
our_rp_xy = field.our_goal.bottom_post.xy
opposing_lp_xy = field.opposing_goal.bottom_post.xy
opposing_rp_xy = field.opposing_goal.top_post.xy
self.our_goal = Goal(name='OurGoal', which_team=OUR_TEAM,
world=self, left_name='OurLeftPost', right_name='OurRightPost',
left_world=our_lp_xy, right_world=our_rp_xy)
self.our_lp = self.our_goal.left
self.our_rp = self.our_goal.right
self.opposing_goal = Goal(name='OpposingGoal', which_team=OPPOSING_TEAM,
world=self, left_name='OpposingLeftPost', right_name='OpposingRightPost',
left_world=opposing_lp_xy, right_world=opposing_rp_xy)
self.opposing_lp = self.opposing_goal.left
self.opposing_rp = self.opposing_goal.right
self.all_posts = set([self.our_lp, self.our_rp, self.opposing_lp, self.opposing_rp])
self.all_posts_hack = [self.our_lp, self.our_rp, self.opposing_lp, self.opposing_rp]
for name in VISION_POSTS_NAMES:
self.addMemoryVars(GoalPost.getVarsForName(name))
# TODO - other robots
# Buttons, Leds (TODO: sonar,
self.robot = Robot(self)
# TODO - is computed used? should be renamed for legibility
self.computed = Computed(self)
# Self orientation and Location of self and other objects in field.
# (passes some stuff into other objects)
self.pose = Pose(self)
self.localization = Localization(self)
self.odometry = Odometry(self)
# The Game-Status, Game-Controller and RobotData Trifecta # TODO: This is messy.
self.playerSettings = PlayerSettings(self) # Start with the default settings. You will be configured later to the right ones by the referees.
self.gameStatus = GameStatus(self, self.playerSettings)
self._gameController = GameController(self.gameStatus)
self._movecoordinator = MoveCoordinator(self)
# All objects that we delegate the event computation and naoqi
# interaction to. TODO: we have the exact state of B-HUMAN, so we
# could use exactly their solution, and hence this todo. We have
# multiple objects that calculate their events based on ground truths
# (naoqi proxies) and on calculated truths. We need to rerun them
# every time something is updated, *in the correct order*. So right
# now I'm hardcoding this by using an ordered list of lists, but
# later this should be calculated by storing a "needed" and
# "provided" list, just like B-HUMAN, and doing the sorting once (and
# that value can be cached to avoid recomputing on each run).
self._objects = [
# All basic objects that rely on just naoproxies should be in the
# first list
[self._movecoordinator,
self.ball, self.our_goal, self.opposing_goal,
self.robot, self._gameController],
[self.gameStatus],
# anything that relies on basics but nothing else should go next
[self],
# self.computed should always be last
[self.localization],
[self.computed],
]
# Another useful collection
self.vision_objects = [self.ball, self.our_lp, self.our_rp, self.opposing_lp, self.opposing_rp]
# logging variables
self._logged_objects = [[obj, None] for obj in [self.ball]]
self._object_to_filename = {self.ball: 'ball'}
self._do_log_positions = burst.options.log_positions
if self._do_log_positions:
self._openPositionLogs()
if burst.options.no_memory_updates:
print "world: NO MEMORY UPDATES.. N-O M-E-M-O-R-Y U-P-D-A-T-E-S"
self._updateMemoryVariables = self._updateMemoryVariables_noop
else:
# Try using shared memory to access variables
if World.running_on_nao:
#self.updateMmapVariablesFile() # // TODO -remove the file! it is EVIL
SharedMemoryReader.tryToInitMMap()
if SharedMemoryReader.isMMapAvailable():
print "world: using SharedMemoryReader"
if US_DISTANCES_VARNAME in self._vars_to_get_list:
self._vars_to_get_list.remove(US_DISTANCES_VARNAME)
self._shm = SharedMemoryReader(self._vars_to_get_list)
self._updateMemoryVariables = self._updateMemoryVariables_noop #(temp)
self._shm.openDeferred.addCallback(self._switchToSharedMemory).addErrback(log.err)
if self._shm is None:
print "world: using ALMemory"
self.checkManModule()
def isSafeToGetUp():
memory = burst.getMemoryProxy()
x = memory.getData("Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value", 0)
return abs(x) < 0.5
try:
period = 2000 # minimum should be 240ms according to documentation
alsonarProxy.subscribe("test", [ period ] )
print "subscription to ALSonar is ok"
except RuntimeError,e:
print "error while subscribing to alsonar"
exit(1)
# processing
# ....
print "processing"
# ====================
# Create proxy to ALMemory
memoryProxy = burst.getMemoryProxy() #could have also used broker.getMemoryProxy()
# Get The Left Foot Force Sensor Values
#for i in xrange(1,2000):
US = memoryProxy.getData("extractors/alsonar/distances",0)
print US
#~ # unsubscribe to ALUltraound
#~ try:
#~ alsonarProxy.unsubscribe("test")
#~ print "unsubscription to ALSonar is ok"
#~ except RuntimeError,e:
#~ print "error while unsubscribing to alsonar"
#~ exit(1)
def isOnBelly():
memory = burst.getMemoryProxy()
y = memory.getData("Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value", 0)
return y > 1.0