Source code for morse.middleware.moos.pose

import logging; logger = logging.getLogger("morse." + __name__)
import pymoos.MOOSCommClient
from morse.middleware.moos import AbstractMOOS
from morse.core import blenderapi

[docs]class PoseNotifier(AbstractMOOS): """ Notify Pose """
[docs] def default(self, ci='unused'): cur_time=pymoos.MOOSCommClient.MOOSTime() # post the simulation time so that it can be synced to MOOSTime self.m.Notify('actual_time', blenderapi.persistantstorage().time.time, cur_time) # post the robot position self.m.Notify('simEast', self.data['x'], cur_time) self.m.Notify('simNorth', self.data['y'], cur_time) self.m.Notify('simHeight', self.data['z'], cur_time) self.m.Notify('simYaw', self.data['yaw'], cur_time) self.m.Notify('simRoll', self.data['roll'], cur_time) self.m.Notify('simPitch', self.data['pitch'], cur_time)
[docs]class PoseReader(AbstractMOOS): """ Read pose commands and update local data. """
[docs] def initialize(self): AbstractMOOS.initialize(self) # register for position variables from the database self.m.Register("pX") self.m.Register("pY") self.m.Register("pZ") self.m.Register("pRoll") self.m.Register("pPitch") self.m.Register("pYaw")
[docs] def default(self, ci='unused'): current_time = pymoos.MOOSCommClient.MOOSTime() # get latest mail from the MOOS comm client messages = self.getRecentMail() new_information = False for message in messages: # look for position messages if (message.GetKey() == "pX") and (message.IsDouble()): self.data['x'] = message.GetDouble() # robot X position [m] new_information = True elif (message.GetKey() == "pY") and (message.IsDouble()): self.data['y'] = message.GetDouble() # robot Y position [m] new_information = True elif (message.GetKey() == "pZ") and (message.IsDouble()): self.data['z'] = message.GetDouble() # robot Z position [m] new_information = True elif (message.GetKey() == "pRoll") and (message.IsDouble()): self.data['roll'] = message.GetDouble() # robot roll [rad] new_information = True elif (message.GetKey() == "pPitch") and (message.IsDouble()): self.data['pitch'] = message.GetDouble() # robot pitch [rad] new_information = True elif (message.GetKey() == "pYaw") and (message.IsDouble()): self.data['yaw'] = message.GetDouble() # robot yaw [rad] new_information = True return new_information