Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 import rospy
00020 from naoqi_driver.naoqi_node import NaoqiNode
00021 from std_srvs.srv import (
00022 SetBoolResponse,
00023 SetBool,
00024 TriggerResponse,
00025 Trigger
00026 )
00027 from distutils.version import LooseVersion
00028
00029 class NaoqiBackgroundMovement(NaoqiNode):
00030 def __init__(self):
00031 NaoqiNode.__init__(self, 'naoqi_background_movement')
00032 self.connectNaoQi()
00033
00034 self.SetEnabledSrv = rospy.Service("background_movement/set_enabled", SetBool, self.handleSetEnabledSrv)
00035 self.IsEnabledSrv = rospy.Service("background_movement/is_enabled", Trigger, self.handleIsEnabledSrv)
00036 rospy.loginfo("naoqi_background_movement initialized")
00037
00038 def connectNaoQi(self):
00039 rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00040 self.systemProxy = self.get_proxy("ALSystem")
00041 if self.systemProxy is None:
00042 rospy.logerr("Could not get a proxy to ALSystem")
00043 exit(1)
00044 else:
00045 if LooseVersion(self.systemProxy.systemVersion()) < LooseVersion("2.4"):
00046 rospy.logerr("Naoqi version of your robot is " + str(self.systemProxy.systemVersion()) + ", which doesn't have a proxy to ALBackgroundMovement.")
00047 exit(1)
00048 else:
00049 self.backgroundMovementProxy = self.get_proxy("ALBackgroundMovement")
00050 if self.backgroundMovementProxy is None:
00051 rospy.logerr("Could not get a proxy to ALBackgroundMovement.")
00052 exit(1)
00053
00054 def handleSetEnabledSrv(self, req):
00055 res = SetBoolResponse()
00056 res.success = False
00057 try:
00058 self.backgroundMovementProxy.setEnabled(req.data)
00059 res.success = True
00060 return res
00061 except RuntimeError, e:
00062 rospy.logerr("Exception caught:\n%s", e)
00063 return res
00064
00065 def handleIsEnabledSrv(self, req):
00066 try:
00067 res = TriggerResponse()
00068 res.success = self.backgroundMovementProxy.isEnabled()
00069 return res
00070 except RuntimeError, e:
00071 rospy.logerr("Exception caught:\n%s", e)
00072 return None
00073
00074 if __name__ == '__main__':
00075 background_movement = NaoqiBackgroundMovement()
00076 rospy.spin()