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 PKG = 'actionlib'
00030
00031 import sys
00032 import unittest
00033 import rospy
00034 import rostest
00035 from actionlib import SimpleActionClient
00036 from actionlib_msgs.msg import *
00037 from actionlib.msg import *
00038
00039
00040 class SimpleExerciser(unittest.TestCase):
00041
00042 def setUp(self):
00043 self.default_wait = rospy.Duration(60.0)
00044 self.client = SimpleActionClient('test_request_action', TestRequestAction)
00045 self.assert_(self.client.wait_for_server(self.default_wait))
00046
00047 def test_just_succeed(self):
00048 goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
00049 the_result = 42)
00050 self.client.send_goal(goal)
00051 self.client.wait_for_result(self.default_wait)
00052
00053 self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state())
00054 self.assertEqual(42, self.client.get_result().the_result)
00055
00056 def test_just_abort(self):
00057 goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_ABORTED,
00058 the_result = 42)
00059 self.client.send_goal(goal)
00060 self.client.wait_for_result(self.default_wait)
00061
00062 self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
00063 self.assertEqual(42, self.client.get_result().the_result)
00064
00065 def test_just_preempt(self):
00066 goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
00067 delay_terminate = rospy.Duration(100000),
00068 the_result = 42)
00069 self.client.send_goal(goal)
00070
00071
00072 timeout_time = rospy.Time.now() + self.default_wait
00073 while rospy.Time.now() < timeout_time:
00074 if (self.client.get_state() != GoalStatus.PENDING):
00075 break
00076 self.client.cancel_goal()
00077
00078 self.client.wait_for_result(self.default_wait)
00079 self.assertEqual(GoalStatus.PREEMPTED, self.client.get_state())
00080 self.assertEqual(42, self.client.get_result().the_result)
00081
00082
00083 def test_drop(self):
00084 goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_DROP,
00085 the_result = 42)
00086 self.client.send_goal(goal)
00087 self.client.wait_for_result(self.default_wait)
00088
00089 self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
00090 self.assertEqual(0, self.client.get_result().the_result)
00091
00092
00093 def test_exception(self):
00094 goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_EXCEPTION,
00095 the_result = 42)
00096 self.client.send_goal(goal)
00097 self.client.wait_for_result(self.default_wait)
00098
00099 self.assertEqual(GoalStatus.ABORTED, self.client.get_state())
00100 self.assertEqual(0, self.client.get_result().the_result)
00101
00102 def test_ignore_cancel_and_succeed(self):
00103 goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
00104 delay_terminate = rospy.Duration(2.0),
00105 ignore_cancel = True,
00106 the_result = 42)
00107 self.client.send_goal(goal)
00108
00109
00110 timeout_time = rospy.Time.now() + self.default_wait
00111 while rospy.Time.now() < timeout_time:
00112 if (self.client.get_state() != GoalStatus.PENDING):
00113 break
00114 self.client.cancel_goal()
00115
00116 self.client.wait_for_result(self.default_wait)
00117
00118 self.assertEqual(GoalStatus.SUCCEEDED, self.client.get_state())
00119 self.assertEqual(42, self.client.get_result().the_result)
00120
00121
00122 def test_lose(self):
00123 goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_LOSE,
00124 the_result = 42)
00125 self.client.send_goal(goal)
00126 self.client.wait_for_result(self.default_wait)
00127
00128 self.assertEqual(GoalStatus.LOST, self.client.get_state())
00129
00130
00131
00132 '''
00133 def test_freeze_server(self):
00134 goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
00135 the_result = 42,
00136 pause_status = rospy.Duration(10.0))
00137 self.client.send_goal(goal)
00138 self.client.wait_for_result(rospy.Duration(13.0))
00139
00140 self.assertEqual(GoalStatus.LOST, self.client.get_state())
00141 '''
00142
00143
00144
00145 if __name__ == '__main__':
00146 rospy.init_node("exercise_simple_server")
00147 rostest.run(PKG, 'exercise_simple_server', SimpleExerciser, sys.argv)