Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 import sys
00030 import time
00031 import rospy
00032 import rostest
00033
00034 from actionlib.simple_action_server import SimpleActionServer
00035 from actionlib.server_goal_handle import ServerGoalHandle;
00036 from actionlib.msg import *
00037
00038 class RefSimpleServer(SimpleActionServer):
00039
00040 def __init__(self, name):
00041 SimpleActionServer.__init__(self, name, TestRequestAction, self.execute_cb, False)
00042 self.start()
00043
00044 def execute_cb(self, goal):
00045 rospy.logdebug("Goal:\n" + str(goal))
00046 result = TestRequestResult(goal.the_result, True)
00047
00048 if goal.pause_status > rospy.Duration(0.0):
00049 rospy.loginfo("Locking the action server for %.3f seconds" % goal.pause_status.to_sec())
00050 status_continue_time = rospy.get_rostime() + goal.pause_status
00051
00052
00053 with self.action_server.lock:
00054 while rospy.get_rostime() < status_continue_time:
00055 time.sleep(0.02)
00056 rospy.loginfo("Unlocking the action server")
00057
00058
00059 terminate_time = rospy.get_rostime() + goal.delay_terminate
00060 while rospy.get_rostime() < terminate_time:
00061 time.sleep(0.02)
00062 if not goal.ignore_cancel:
00063 if self.is_preempt_requested():
00064 self.set_preempted(result, goal.result_text)
00065 return
00066
00067 rospy.logdebug("Terminating goal as: %i" % goal.terminate_status)
00068
00069 if goal.terminate_status == TestRequestGoal.TERMINATE_SUCCESS:
00070 self.set_succeeded(result, goal.result_text)
00071 elif goal.terminate_status == TestRequestGoal.TERMINATE_ABORTED:
00072 self.set_aborted(result, goal.result_text)
00073 elif goal.terminate_status == TestRequestGoal.TERMINATE_REJECTED:
00074 rospy.logerr("Simple action server cannot reject goals")
00075 self.set_aborted(None, "Simple action server cannot reject goals")
00076 elif goal.terminate_status == TestRequestGoal.TERMINATE_DROP:
00077 rospy.loginfo("About to drop the goal. This should produce an error message.")
00078 return
00079 elif goal.terminate_status == TestRequestGoal.TERMINATE_EXCEPTION:
00080 rospy.loginfo("About to throw an exception. This should produce an error message.")
00081 raise Exception("Terminating by throwing an exception")
00082 elif goal.terminate_status == TestRequestGoal.TERMINATE_LOSE:
00083
00084 for i, s in enumerate(self.action_server.status_list):
00085 if s.status.goal_id.id == self.current_goal.goal.goal_id.id:
00086 del self.action_server.status_list[i]
00087 break
00088 self.current_goal = ServerGoalHandle()
00089 else:
00090 rospy.logerr("Don't know how to terminate as %d" % goal.terminate_status)
00091 self.set_aborted(None, "Don't know how to terminate as %d" % goal.terminate_status)
00092
00093
00094
00095 if __name__ == '__main__':
00096 rospy.init_node("ref_simple_server")
00097 ref_server = RefSimpleServer("test_request_action")
00098 print "Spinning"
00099 rospy.spin()