nao_behaviors.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #
00004 # ROS node to control NAO's built-in and user-installed behaviors using NaoQI
00005 # Tested with NaoQI: 1.12
00006 #
00007 # Copyright (c) 2012, 2013 Miguel Sarabia
00008 # Imperial College London
00009 #
00010 # Redistribution and use in source and binary forms, with or without
00011 # modification, are permitted provided that the following conditions are met:
00012 #
00013 #    # Redistributions of source code must retain the above copyright
00014 #       notice, this list of conditions and the following disclaimer.
00015 #    # Redistributions in binary form must reproduce the above copyright
00016 #       notice, this list of conditions and the following disclaimer in the
00017 #       documentation and/or other materials provided with the distribution.
00018 #    # Neither the name of the Imperial College London nor the names of its
00019 #       contributors may be used to endorse or promote products derived from
00020 #       this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00023 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00024 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00025 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00026 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00027 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00028 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00029 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00030 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00031 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 
00035 import threading
00036 import rospy
00037 import actionlib
00038 
00039 from naoqi_driver.naoqi_node import NaoqiNode
00040 
00041 from naoqi_bridge_msgs.msg import RunBehaviorAction
00042 
00043 from naoqi_bridge_msgs.srv import (
00044     GetInstalledBehaviors,
00045     GetInstalledBehaviorsResponse,
00046     )
00047 
00048 class NaoBehaviors(NaoqiNode):
00049     #This should be treated as a constant
00050     NODE_NAME = "nao_behaviors"
00051 
00052     def __init__( self ):
00053 
00054         #Initialisation
00055         NaoqiNode.__init__( self, self.NODE_NAME )
00056 
00057         #We need this variable to be able to call stop behavior when preempted
00058         self.behavior = None
00059         self.lock = threading.RLock()
00060 
00061         #Proxy for listingBehaviors and stopping them
00062         self.behaviorProxy = self.get_proxy( "ALBehaviorManager" )
00063 
00064         # Register ROS services
00065         self.getInstalledBehaviorsService = rospy.Service(
00066             "get_installed_behaviors",
00067             GetInstalledBehaviors,
00068             self.getInstalledBehaviors
00069             )
00070 
00071         #Prepare and start actionlib server
00072         self.actionlibServer = actionlib.SimpleActionServer(
00073             "run_behavior",
00074             RunBehaviorAction,
00075             self.runBehavior,
00076             False
00077             )
00078 
00079         self.actionlibServer.register_preempt_callback( self.stopBehavior )
00080 
00081         self.actionlibServer.start()
00082 
00083     def getInstalledBehaviors( self, request ):
00084         result = self.behaviorProxy.getInstalledBehaviors()
00085         return GetInstalledBehaviorsResponse( result )
00086 
00087 
00088     def runBehavior( self, request ):
00089         #Note this function is executed from a different thread
00090         rospy.logdebug(
00091             "Execution of behavior: '{}' requested".format(request.behavior))
00092 
00093         #Check requested behavior is installed
00094         if not request.behavior in self.behaviorProxy.getInstalledBehaviors():
00095             error_msg = "Behavior '{}' not installed".format(request.behavior)
00096             self.actionlibServer.set_aborted(text = error_msg)
00097             rospy.logdebug(error_msg)
00098             return
00099 
00100         with self.lock:
00101             # Check first if we're already preempted, and return if so
00102             if self.actionlibServer.is_preempt_requested():
00103                 self.actionlibServer.set_preempted()
00104                 rospy.logdebug("Behavior execution preempted before it started")
00105                 return
00106 
00107             #Save name of behavior to be run
00108             self.behavior = request.behavior
00109             #Execute behavior (on another thread so we can release lock)
00110             taskID = self.behaviorProxy.post.runBehavior( self.behavior )
00111 
00112         # Wait for task to complete (or be preempted)
00113         rospy.logdebug("Waiting for behavior execution to complete")
00114         self.behaviorProxy.wait( taskID, 0 )
00115 
00116         #Evaluate results
00117         with self.lock:
00118             self.behavior = None
00119             # If preempted, report so
00120             if self.actionlibServer.is_preempt_requested() :
00121                 self.actionlibServer.set_preempted()
00122                 rospy.logdebug("Behavior execution preempted")
00123             # Otherwise, set as succeeded
00124             else:
00125                 self.actionlibServer.set_succeeded()
00126                 rospy.logdebug("Behavior execution succeeded")
00127 
00128     def stopBehavior( self ):
00129         with self.lock:
00130             if self.behavior and self.actionlibServer.is_active() :
00131                 self.behaviorProxy.stopBehavior( self.behavior )
00132 
00133 
00134 if __name__ == '__main__':
00135     node = NaoBehaviors()
00136     rospy.loginfo( node.NODE_NAME + " running..." )
00137     rospy.spin()
00138     rospy.loginfo( node.NODE_NAME + " stopped." )
00139     exit(0)


nao_apps
Author(s):
autogenerated on Thu Aug 27 2015 14:02:19