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
00030
00031 PKG = 'actionlib'
00032 import rospy
00033
00034 from actionlib.action_server import ActionServer
00035 from actionlib.msg import TestAction, TestFeedback, TestResult
00036
00037
00038 class RefServer (ActionServer):
00039
00040 def __init__(self, name):
00041 action_spec = TestAction
00042 ActionServer.__init__(
00043 self, name, action_spec, self.goalCallback, self.cancelCallback, False)
00044 self.start()
00045 rospy.loginfo("Creating ActionServer [%s]\n", name)
00046
00047 self.saved_goals = []
00048
00049 def goalCallback(self, gh):
00050 goal = gh.get_goal()
00051
00052 rospy.loginfo("Got goal %d", int(goal.goal))
00053 if goal.goal == 1:
00054 gh.set_accepted()
00055 gh.set_succeeded(None, "The ref server has succeeded")
00056 elif goal.goal == 2:
00057 gh.set_accepted()
00058 gh.set_aborted(None, "The ref server has aborted")
00059 elif goal.goal == 3:
00060 gh.set_rejected(None, "The ref server has rejected")
00061 elif goal.goal == 4:
00062 self.saved_goals.append(gh)
00063 gh.set_accepted()
00064 elif goal.goal == 5:
00065 gh.set_accepted()
00066 for g in self.saved_goals:
00067 g.set_succeeded()
00068 self.saved_goals = []
00069 gh.set_succeeded()
00070 elif goal.goal == 6:
00071 gh.set_accepted()
00072 for g in self.saved_goals:
00073 g.set_aborted()
00074 self.saved_goals = []
00075 gh.set_succeeded()
00076 elif goal.goal == 7:
00077 gh.set_accepted()
00078 n = len(self.saved_goals)
00079 for i, g in enumerate(self.saved_goals):
00080 g.publish_feedback(TestFeedback(n-i))
00081 gh.set_succeeded()
00082 elif goal.goal == 8:
00083 gh.set_accepted()
00084 n = len(self.saved_goals)
00085 for i, g in enumerate(self.saved_goals):
00086 if i % 2 == 0:
00087 g.set_succeeded(TestResult(n - i), "The ref server has succeeded")
00088 else:
00089 g.set_aborted(TestResult(n - i), "The ref server has aborted")
00090 self.saved_goals = []
00091 gh.set_succeeded()
00092 else:
00093 pass
00094
00095 def cancelCallback(self, gh):
00096 pass
00097
00098
00099 if __name__ == "__main__":
00100 rospy.init_node("ref_server")
00101 ref_server = RefServer("reference_action")
00102
00103 rospy.spin()