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