29 from threading
import Thread
35 from naoqi
import ALProxy
37 raise RuntimeError(
"Error importing NaoQI. Please make sure that Aldebaran's NaoQI API is in your PYTHONPATH.")
41 A ROS Node wrapper that can help you connect to NAOqi and deal with ROS shutdown 42 To start your node, just call: 43 my_node = MyNode('my_node') 44 my_node.start() # that will spawn your node in a thread (and run whatever is in the run() function 46 # when killing ROS, the node will automatically stop its main loop, exit, and then unsubscribe from ALMemory events 47 # and call whatever you have in unsubscribe() 48 Then, if your node needs to process data, you just needs to have a run function: 51 #do some initialization 52 while self.is_looping(): 54 # do some post processing 58 :param name: the name of the ROS node 71 rospy.init_node(self.
__name)
74 default_ip = rospy.get_param(
"~pip",
"127.0.0.1")
75 default_port = rospy.get_param(
"~pport", 9559)
78 from argparse
import ArgumentParser
79 parser = ArgumentParser()
80 parser.add_argument(
"--pip", dest=
"pip", default=default_ip,
81 help=
"IP/hostname of parent broker. Default is 127.0.0.1.", metavar=
"IP")
82 parser.add_argument(
"--pport", dest=
"pport", default=default_port, type=int,
83 help=
"port of parent broker. Default is 9559.", metavar=
"PORT")
86 args, unknown = parser.parse_known_args(args=rospy.myargv(argv=sys.argv)[1:])
97 Callback function called whenever rospy.spin() stops 99 rospy.loginfo(
'Stopping ' + self.
__name)
106 rospy.loginfo(self.
__name +
' stopped')
110 This is a virtual method that corresponds to the code of the Node that runs continuously 111 It should have a while loop calling the self.is_looping() function 115 #do some initialization 116 while self.is_looping(): 118 # do some post processing 120 raise NotImplementedError(
'Implement the run function of your NaoNode !')
124 :return: whether the thread is supposed to be running 130 Returns a proxy to a specific module. If it has not been created yet, it is created 131 :param name: the name of the module to create a proxy for 132 :return: a proxy to the corresponding module 139 proxy = ALProxy(name,self.
pip,self.
pport)
140 except RuntimeError,e:
142 rospy.logerr(
"Could not create Proxy to \"%s\". \nException message:\n%s",name, e)
149 Returns the NAOqi version. 150 A proxy for ALMemory is automatically created if needed as self.memProxy. 151 You can then simply have code that runs or not depending on the NAOqi version. 152 E.g. if distutils.version.LooseVersion('1.6') < get_version() .... 153 :return: a distutils.version.LooseVersion object with the NAOqi version 164 from distutils.version
import LooseVersion
def __on_ros_shutdown(self)
__proxies
NAOqi stuff dict from a modulename to a proxy.
def get_proxy(self, name, warn=True)