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