nao_driver_naoqi.py
Go to the documentation of this file.
00001 # Copyright 2009-2011 Armin Hornung, University of Freiburg
00002 # http://www.ros.org/wiki/nao
00003 #
00004 # Redistribution and use in source and binary forms, with or without
00005 # modification, are permitted provided that the following conditions are met:
00006 #
00007 #    # Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #    # Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #    # Neither the name of the University of Freiburg nor the names of its
00013 #       contributors may be used to endorse or promote products derived from
00014 #       this software without specific prior written permission.
00015 #
00016 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00017 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00018 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00019 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00020 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00021 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00022 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00023 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00024 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00025 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00026 # POSSIBILITY OF SUCH DAMAGE.
00027 #
00028 
00029 from threading import Thread
00030 
00031 import rospy
00032 
00033 # import Aldebaran API (must be in PYTHONPATH):
00034 try:
00035     from naoqi import ALProxy
00036 except ImportError:
00037     raise RuntimeError("Error importing NaoQI. Please make sure that Aldebaran's NaoQI API is in your PYTHONPATH.")
00038 
00039 class NaoNode(Thread):
00040     """
00041     A ROS Node wrapper that can help you connect to NAOqi and deal with ROS shutdown
00042     To start your node, just call:
00043     my_node = MyNode('my_node')
00044     my_node.start() # that will spawn your node in a thread (and run whatever is in the run() function
00045     rospy.spin()
00046     # when killing ROS, the node will automatically stop its main loop, exit, and then unsubscribe from ALMemory events
00047     # and call whatever you have in unsubscribe()
00048     Then, if your node needs to process data, you just needs to have a run function:
00049 
00050     def run(Self):
00051         #do some initialization
00052         while self.is_looping():
00053             # do something
00054         # do some post processing
00055     """
00056     def __init__(self, name):
00057         """
00058         :param name: the name of the ROS node
00059         """
00060         super(NaoNode, self).__init__()
00061 
00062         # A distutils.version.LooseVersion that contains the current verssion of NAOqi we're connected to
00063         self.__naoqi_version = None
00064         self.__name = name
00065 
00066         ## NAOqi stuff
00067         # dict from a modulename to a proxy
00068         self.__proxies = {}
00069 
00070         # If user has set parameters for ip and port use them as default
00071         default_ip = rospy.get_param("~pip", "127.0.0.1")
00072         default_port = rospy.get_param("~pport", 9559)
00073 
00074         # get connection from command line:
00075         from argparse import ArgumentParser
00076         parser = ArgumentParser()
00077         parser.add_argument("--pip", dest="pip", default=default_ip,
00078                           help="IP/hostname of parent broker. Default is 127.0.0.1.", metavar="IP")
00079         parser.add_argument("--pport", dest="pport", default=default_port, type=int,
00080                           help="port of parent broker. Default is 9559.", metavar="PORT")
00081 
00082         import sys
00083         args = parser.parse_args(args=rospy.myargv(argv=sys.argv)[1:])
00084         self.pip = args.pip
00085         self.pport = args.pport
00086 
00087         ## ROS stuff
00088         self.__stop_thread = False
00089         rospy.init_node(self.__name)
00090         # make sure that we unregister from everything when the module dies
00091         rospy.on_shutdown(self.__on_ros_shutdown)
00092 
00093     def __on_ros_shutdown(self):
00094         """
00095         Callback function called whenever rospy.spin() stops
00096         """
00097         rospy.loginfo('Stopping ' + self.__name)
00098 
00099         self.__stop_thread = True
00100         # wait for the thread to be done
00101         if self.is_alive():
00102             self.join()
00103 
00104         rospy.loginfo(self.__name + ' stopped')
00105 
00106     def run(self):
00107         """
00108         This is a virtual method that corresponds to the code of the Node that runs continuously
00109         It should have a while loop calling the self.is_looping() function
00110         """
00111         """
00112         # code example
00113         #do some initialization
00114         while self.is_looping():
00115             # do something
00116         # do some post processing
00117         """
00118         raise NotImplementedError('Implement the run function of your NaoNode !')
00119 
00120     def is_looping(self):
00121         """
00122         :return: whether the thread is supposed to be running
00123         """
00124         return not self.__stop_thread
00125 
00126     def get_proxy(self, name, warn=True):
00127         """
00128         Returns a proxy to a specific module. If it has not been created yet, it is created
00129         :param name: the name of the module to create a proxy for
00130         :return: a proxy to the corresponding module
00131         """
00132         if name in self.__proxies and self.__proxies[name] is not None:
00133             return self.__proxies[name]
00134 
00135         proxy = None
00136         try:
00137             proxy = ALProxy(name,self.pip,self.pport)
00138         except RuntimeError,e:
00139             if warn:
00140                 rospy.logerr("Could not create Proxy to \"%s\". \nException message:\n%s",name, e)
00141 
00142         self.__proxies[name] = proxy
00143         return proxy
00144 
00145     def get_version(self):
00146         """
00147         Returns the NAOqi version.
00148         A proxy for ALMemory is automatically created if needed as self.memProxy.
00149         You can then simply have code that runs or not depending on the NAOqi version.
00150         E.g. if distutils.version.LooseVersion('1.6') < get_version()    ....
00151         :return: a distutils.version.LooseVersion object with the NAOqi version
00152         """
00153         if self.__naoqi_version is None:
00154             proxy = self.get_proxy('ALMemory')
00155             if proxy is None:
00156                 # exiting is bad but it should not happen
00157                 # except maybe with NAOqi versions < 1.6 or future ones
00158                 # in which case we will adapt that code to call the proper
00159                 # version function
00160                 exit(1)
00161 
00162             from distutils.version import LooseVersion
00163             self.__naoqi_version = LooseVersion(proxy.version())
00164 
00165         return self.__naoqi_version


nao_driver
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Séverin Lemaignan
autogenerated on Mon Oct 6 2014 02:39:17