nao_leds.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #
00004 # ROS node to control NAO's LEDs 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 
00036 import rospy
00037 import actionlib
00038 import random
00039 import copy
00040 
00041 from naoqi_driver.naoqi_node import NaoqiNode
00042 
00043 from naoqi_bridge_msgs.msg import(
00044     BlinkAction,
00045     BlinkResult,
00046     BlinkFeedback,
00047     FadeRGB
00048     )
00049 
00050 class NaoLeds(NaoqiNode):
00051     #This should be treated as a constant
00052     NODE_NAME = "nao_leds"
00053 
00054     def __init__( self ):
00055 
00056         #Initialisation
00057         NaoqiNode.__init__( self, self.NODE_NAME )
00058 
00059         #Proxy to interface with LEDs
00060         self.proxy = self.get_proxy( "ALLeds" )
00061 
00062         #Seed python's random number generator
00063         random.seed( rospy.Time.now().to_nsec() )
00064 
00065         #Create a subscriber for the fade_rgb topic
00066         self.subscriber = rospy.Subscriber(
00067             "fade_rgb",
00068             FadeRGB,
00069             self.fade_rgb)
00070 
00071         #Prepare and start actionlib server
00072         self.actionlib = actionlib.SimpleActionServer(
00073             "blink",
00074             BlinkAction,
00075             self.run_blink,
00076             False
00077             )
00078         self.actionlib.start()
00079 
00080     def fade_rgb(self, request) :
00081         hexColor =  int(
00082             int(request.color.r*255) << 16 |
00083             int(request.color.g*255) << 8 |
00084             int(request.color.b*255)
00085             )
00086 
00087         self.proxy.fadeRGB(
00088             request.led_name,
00089             hexColor,
00090             request.fade_duration.to_sec()
00091             )
00092 
00093     def run_blink( self, request ):
00094         #Note that this function is executed on a different thread
00095 
00096         third_of_duration = request.blink_duration / 3.0
00097 
00098         #Prepare background message
00099         bg_msg = FadeRGB();
00100         bg_msg.led_name = "FaceLeds"
00101         bg_msg.color = request.bg_color
00102         bg_msg.fade_duration = third_of_duration
00103 
00104         #Prepare a copy for blink_msg
00105         blink_msg = copy.deepcopy( bg_msg )
00106 
00107         #Construct result and feedback
00108         feedback = BlinkFeedback()
00109         result = BlinkResult()
00110         result.still_blinking = False
00111 
00112         #Check valid parameters
00113         bad_request = False
00114         reason = ""
00115         if not request.colors:
00116             bad_request = True
00117             reason = "No colors to blink were specified"
00118         elif request.blink_duration.to_sec() <= 0.0:
00119             bad_request = True
00120             reason = "Blink duration cannot be 0"
00121         elif request.blink_rate_mean <= 0.0 or request.blink_rate_sd <= 0.0:
00122             bad_request = True
00123             reason = "Invalid parameter for blink rate"
00124 
00125         if bad_request:
00126             rospy.logwarn("Bad Blink request: {}".format(reason))
00127             self.actionlib.set_aborted(result, reason)
00128             return
00129 
00130         #Sleep time is drawn from a gaussian dist with these parameters
00131         sleep_mean = request.blink_rate_mean
00132         sleep_sd = request.blink_rate_sd
00133         max_sleep_time = sleep_mean + 3* sleep_sd #This is highly unlikely
00134 
00135         #Main blinking loop
00136         while ( not self.actionlib.is_preempt_requested() and
00137             not rospy.is_shutdown() ) :
00138 
00139             #Chose a blinking color at random
00140             blink_msg.color = random.choice( request.colors )
00141 
00142             #Send command (takes 1/3 duration to fade)
00143             self.fade_rgb( blink_msg )
00144             #Sleep (takes 1/3 duration)
00145             rospy.sleep( third_of_duration )
00146             #Fade to background (takes another 1/3 duration)
00147             self.fade_rgb( bg_msg )
00148 
00149             #Publish feedback
00150             feedback.last_color = blink_msg.color
00151             self.actionlib.publish_feedback( feedback )
00152 
00153             #Sleep for a random amount of time
00154             sleep_time = random.gauss( sleep_mean, sleep_sd )
00155 
00156             if sleep_time < 0:
00157                 sleep_time = 0
00158             elif sleep_time > max_sleep_time :
00159                 sleep_time = max_sleep_time
00160 
00161             rospy.sleep( sleep_time )
00162 
00163         #If we were pre-empted by other request, make sure result shows this
00164         if self.actionlib.is_new_goal_available() :
00165             result.still_blinking = True
00166 
00167         # Task never completes so if we reach here, blink has been pre-empted
00168         self.actionlib.set_preempted( result )
00169 
00170 if __name__ == '__main__':
00171     node = NaoLeds()
00172     rospy.loginfo( node.NODE_NAME + " running..." )
00173     rospy.spin()
00174     rospy.loginfo( node.NODE_NAME + " stopped." )
00175     exit(0)


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