00001 #! /usr/bin/env python 00002 00003 # Copyright (c) 2010, Washington University in St. Louis 00004 # All rights reserved. 00005 # 00006 # Redistribution and use in source and binary forms, with or without 00007 # modification, are permitted provided that the following conditions are met: 00008 # 00009 # * Redistributions of source code must retain the above copyright 00010 # notice, this list of conditions and the following disclaimer. 00011 # * Redistributions in binary form must reproduce the above copyright 00012 # notice, this list of conditions and the following disclaimer in the 00013 # documentation and/or other materials provided with the distribution. 00014 # * Neither the name of the Washington University in St. Louis nor the 00015 # names of its contributors may be used to endorse or promote products 00016 # derived from this software without specific prior written permission. 00017 # 00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00028 # POSSIBILITY OF SUCH DAMAGE. 00029 00030 import roslib; roslib.load_manifest('actionlib_tutorials') 00031 import rospy 00032 00033 import actionlib 00034 00035 import actionlib_tutorials.msg 00036 00037 class FibonacciAction(object): 00038 # create messages that are used to publish feedback/result 00039 _feedback = actionlib_tutorials.msg.FibonacciFeedback() 00040 _result = actionlib_tutorials.msg.FibonacciResult() 00041 00042 def __init__(self, name): 00043 self._action_name = name 00044 self._as = actionlib.SimpleActionServer(self._action_name, actionlib_tutorials.msg.FibonacciAction, execute_cb=self.execute_cb) 00045 00046 def execute_cb(self, goal): 00047 # helper variables 00048 r = rospy.Rate(1) 00049 success = True 00050 00051 # append the seeds for the fibonacci sequence 00052 self._feedback.sequence = [] 00053 self._feedback.sequence.append(0) 00054 self._feedback.sequence.append(1) 00055 00056 # publish info to the console for the user 00057 rospy.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self._action_name, goal.order, self._feedback.sequence[0], self._feedback.sequence[1])) 00058 00059 # start executing the action 00060 for i in xrange(1, goal.order): 00061 # check that preempt has not been requested by the client 00062 if self._as.is_preempt_requested(): 00063 rospy.loginfo('%s: Preempted' % self._action_name) 00064 self._as.set_preempted() 00065 success = False 00066 break 00067 self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1]) 00068 # publish the feedback 00069 self._as.publish_feedback(self._feedback) 00070 # this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes 00071 r.sleep() 00072 00073 if success: 00074 self._result.sequence = self._feedback.sequence 00075 rospy.loginfo('%s: Succeeded' % self._action_name) 00076 self._as.set_succeeded(self._result) 00077 00078 if __name__ == '__main__': 00079 rospy.init_node('fibonacci') 00080 FibonacciAction(rospy.get_name()) 00081 rospy.spin()