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 from actionlib_msgs.msg import *
00035 from actionlib import SimpleActionClient
00036 from actionlib import ActionClient
00037 from actionlib.msg import TestAction, TestGoal
00038
00039 class TestRefSimpleActionServer(unittest.TestCase):
00040
00041 def testsimple(self):
00042 return
00043 client = ActionClient('reference_action', TestAction)
00044 self.assert_(client.wait_for_action_server_to_start(rospy.Duration(2.0)),
00045 'Could not connect to the action server')
00046
00047 goal = TestGoal(1)
00048 client.send_goal(goal)
00049 self.assert_(client.wait_for_goal_to_finish(rospy.Duration(2.0)),
00050 "Goal didn't finish")
00051 self.assertEqual(GoalStatus.SUCCEEDED, client.get_terminal_state())
00052 self.assertEqual(GoalStatus.SUCCEEDED, client.get_state())
00053
00054 goal = TestGoal(2)
00055 client.send_goal(goal)
00056 self.assert_(client.wait_for_goal_to_finish(rospy.Duration(10.0)),
00057 "Goal didn't finish")
00058 self.assertEqual(GoalStatus.ABORTED, client.get_terminal_state())
00059 self.assertEqual(GoalStatus.ABORTED, client.get_state())
00060
00061
00062 def test_abort(self):
00063 client = ActionClient('reference_action', TestAction)
00064 self.assert_(client.wait_for_server(rospy.Duration(2.0)),
00065 'Could not connect to the action server')
00066
00067 goal_work = TestGoal(4)
00068 goal_abort = TestGoal(6)
00069 goal_feedback = TestGoal(8)
00070
00071 g1=client.send_goal(goal_work)
00072 g2=client.send_goal(goal_work)
00073 g3=client.send_goal(goal_work)
00074 g4=client.send_goal(goal_work)
00075
00076 rospy.sleep(0.5);
00077 self.assertEqual(g1.get_goal_status(),GoalStatus.ACTIVE)
00078 self.assertEqual(g2.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
00079 self.assertEqual(g3.get_goal_status(),GoalStatus.ACTIVE,"Shoule be active")
00080 self.assertEqual(g4.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
00081
00082 g5=client.send_goal(goal_abort)
00083 rospy.sleep(0.5);
00084 self.assertEqual(g5.get_goal_status(),GoalStatus.SUCCEEDED,"Should be done")
00085
00086 self.assertEqual(g1.get_goal_status(),GoalStatus.ABORTED,"Should be aborted")
00087 self.assertEqual(g2.get_goal_status(),GoalStatus.ABORTED,"Should be aborted")
00088 self.assertEqual(g3.get_goal_status(),GoalStatus.ABORTED,"Shoule be aborted")
00089 self.assertEqual(g4.get_goal_status(),GoalStatus.ABORTED,"Should be aborted")
00090
00091
00092
00093 def test_feedback(self):
00094 client = ActionClient('reference_action', TestAction)
00095 self.assert_(client.wait_for_server(rospy.Duration(2.0)),
00096 'Could not connect to the action server')
00097
00098 goal_work = TestGoal(4)
00099 goal_abort = TestGoal(6)
00100 goal_feedback = TestGoal(7)
00101
00102 rospy.logwarn("This is a hacky way to associate goals with feedback");
00103 feedback={};
00104 def update_feedback(id,g,f):
00105 feedback[id]=f;
00106
00107 g1=client.send_goal(goal_work,feedback_cb=lambda g,f:update_feedback(0,g,f))
00108 g2=client.send_goal(goal_work,feedback_cb=lambda g,f:update_feedback(1,g,f))
00109 g3=client.send_goal(goal_work,feedback_cb=lambda g,f:update_feedback(2,g,f))
00110 g4=client.send_goal(goal_work,feedback_cb=lambda g,f:update_feedback(3,g,f))
00111
00112 rospy.sleep(0.5);
00113 self.assertEqual(g1.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
00114 self.assertEqual(g2.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
00115 self.assertEqual(g3.get_goal_status(),GoalStatus.ACTIVE,"Shoule be active")
00116 self.assertEqual(g4.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
00117
00118 g5=client.send_goal(goal_feedback)
00119 rospy.sleep(0.5);
00120 self.assertEqual(g5.get_goal_status(),GoalStatus.SUCCEEDED,"Should be done")
00121
00122
00123 self.assertEqual(g1.get_goal_status(),GoalStatus.ACTIVE)
00124 self.assertEqual(feedback[0].feedback,4)
00125 self.assertEqual(g2.get_goal_status(),GoalStatus.ACTIVE)
00126 self.assertEqual(feedback[1].feedback,3)
00127 self.assertEqual(g3.get_goal_status(),GoalStatus.ACTIVE)
00128 self.assertEqual(feedback[2].feedback,2)
00129 self.assertEqual(g4.get_goal_status(),GoalStatus.ACTIVE)
00130 self.assertEqual(feedback[3].feedback,1)
00131
00132 g6=client.send_goal(goal_abort)
00133 rospy.sleep(0.5);
00134
00135
00136 def test_result(self):
00137 client = ActionClient('reference_action', TestAction)
00138 self.assert_(client.wait_for_server(rospy.Duration(2.0)),
00139 'Could not connect to the action server')
00140
00141 goal_work = TestGoal(4)
00142 goal_abort = TestGoal(6)
00143 goal_result = TestGoal(8)
00144
00145 rospy.logwarn("This is a hacky way to associate goals with feedback");
00146
00147 g1=client.send_goal(goal_work)
00148 g2=client.send_goal(goal_work)
00149 g3=client.send_goal(goal_work)
00150 g4=client.send_goal(goal_work)
00151
00152 rospy.sleep(0.5);
00153 self.assertEqual(g1.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
00154 self.assertEqual(g2.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
00155 self.assertEqual(g3.get_goal_status(),GoalStatus.ACTIVE,"Shoule be active")
00156 self.assertEqual(g4.get_goal_status(),GoalStatus.ACTIVE,"Should be active")
00157
00158 g5=client.send_goal(goal_result)
00159 rospy.sleep(0.5);
00160 self.assertEqual(g5.get_goal_status(),GoalStatus.SUCCEEDED,"Should be done")
00161
00162 self.assertEqual(g1.get_goal_status(),GoalStatus.SUCCEEDED)
00163 self.assertEqual(g1.get_result().result,4)
00164 self.assertEqual(g2.get_goal_status(),GoalStatus.ABORTED)
00165 self.assertEqual(g2.get_result().result,3)
00166 self.assertEqual(g3.get_goal_status(),GoalStatus.SUCCEEDED)
00167 self.assertEqual(g3.get_result().result,2)
00168 self.assertEqual(g4.get_goal_status(),GoalStatus.ABORTED)
00169 self.assertEqual(g4.get_result().result,1)
00170 g6=client.send_goal(goal_abort)
00171 rospy.sleep(0.5);
00172
00173
00174
00175
00176
00177 if __name__ == '__main__':
00178 import rostest
00179 rospy.init_node('test_ref_simple_action_server')
00180 rostest.rosrun('actionlib', 'test_simple_action_client_python', TestRefSimpleActionServer)