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 nao_driver import NaoNode
00042 
00043 from nao_msgs.msg import(
00044     BlinkAction,
00045     BlinkResult,
00046     BlinkFeedback,
00047     FadeRGB
00048     )
00049 
00050 class NaoLeds(NaoNode):
00051     #This should be treated as a constant
00052     NODE_NAME = "nao_leds"
00053     
00054     def __init__( self ):
00055         
00056         #Initialisation
00057         NaoNode.__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     
00081     def fade_rgb(self, request) :
00082         hexColor =  int(
00083             int(request.color.r*255) << 16 | 
00084             int(request.color.g*255) << 8 |
00085             int(request.color.b*255)
00086             )
00087 
00088         self.proxy.fadeRGB(
00089             request.led_name,
00090             hexColor,
00091             request.fade_duration.to_sec()
00092             )
00093     
00094     def run_blink( self, request ):
00095         #Note that this function is executed on a different thread
00096 
00097         third_of_duration = request.blink_duration / 3.0
00098         
00099         #Prepare background message
00100         bg_msg = FadeRGB();
00101         bg_msg.led_name = "FaceLeds"
00102         bg_msg.color = request.bg_color
00103         bg_msg.fade_duration = third_of_duration
00104         
00105         #Prepare a copy for blink_msg
00106         blink_msg = copy.deepcopy( bg_msg )
00107         
00108         #Construct result and feedback
00109         feedback = BlinkFeedback()
00110         result = BlinkResult()
00111         result.still_blinking = False
00112 
00113         #Check valid parameters
00114         bad_request = False
00115         reason = ""
00116         if not request.colors:
00117             bad_request = True
00118             reason = "No colors to blink were specified"
00119         elif request.blink_duration.to_sec() <= 0.0:
00120             bad_request = True
00121             reason = "Blink duration cannot be 0"
00122         elif request.blink_rate_mean <= 0.0 or request.blink_rate_sd <= 0.0:
00123             bad_request = True
00124             reason = "Invalid parameter for blink rate"
00125         
00126         if bad_request:
00127             rospy.logwarn("Bad Blink request: {}".format(reason))
00128             self.actionlib.set_aborted(result, reason)
00129             return
00130         
00131         #Sleep time is drawn from a gaussian dist with these parameters
00132         sleep_mean = request.blink_rate_mean
00133         sleep_sd = request.blink_rate_sd
00134         max_sleep_time = sleep_mean + 3* sleep_sd #This is highly unlikely
00135         
00136         #Main blinking loop
00137         while ( not self.actionlib.is_preempt_requested() and 
00138             not rospy.is_shutdown() ) :
00139             
00140             #Chose a blinking color at random
00141             blink_msg.color = random.choice( request.colors )
00142 
00143             #Send command (takes 1/3 duration to fade)
00144             self.fade_rgb( blink_msg )
00145             #Sleep (takes 1/3 duration)
00146             rospy.sleep( third_of_duration )
00147             #Fade to background (takes another 1/3 duration)
00148             self.fade_rgb( bg_msg )
00149             
00150             #Publish feedback
00151             feedback.last_color = blink_msg.color
00152             self.actionlib.publish_feedback( feedback )
00153             
00154             #Sleep for a random amount of time
00155             sleep_time = random.gauss( sleep_mean, sleep_sd )
00156             
00157             if sleep_time < 0:
00158                 sleep_time = 0
00159             elif sleep_time > max_sleep_time : 
00160                 sleep_time = max_sleep_time
00161                 
00162             rospy.sleep( sleep_time )
00163         
00164         #If we were pre-empted by other request, make sure result shows this
00165         if self.actionlib.is_new_goal_available() :
00166             result.still_blinking = True
00167                 
00168         # Task never completes so if we reach here, blink has been pre-empted
00169         self.actionlib.set_preempted( result )
00170 
00171 if __name__ == '__main__':
00172     node = NaoLeds()
00173     rospy.loginfo( node.NODE_NAME + " running..." )
00174     rospy.spin()
00175     rospy.loginfo( node.NODE_NAME + " stopped." )
00176     exit(0)


nao_driver
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Séverin Lemaignan
autogenerated on Thu Oct 30 2014 09:20:04