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