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 PKG='actionlib'
00029
00030 import sys
00031 import unittest
00032 import rospy
00033 from actionlib_msgs.msg import *
00034 from actionlib import SimpleActionClient
00035 from actionlib import ActionClient
00036 from actionlib.msg import TestAction, TestGoal
00037
00038 class TestRefSimpleActionServer(unittest.TestCase):
00039
00040 def test_one(self):
00041 client = SimpleActionClient('reference_simple_action', TestAction)
00042 self.assert_(client.wait_for_server(rospy.Duration(2.0)),
00043 'Could not connect to the action server')
00044
00045 goal = TestGoal(1)
00046 client.send_goal(goal)
00047 self.assert_(client.wait_for_result(rospy.Duration(2.0)),
00048 "Goal didn't finish")
00049 self.assertEqual(GoalStatus.SUCCEEDED, client.get_state())
00050
00051 goal = TestGoal(2)
00052 client.send_goal(goal)
00053 self.assert_(client.wait_for_result(rospy.Duration(10.0)),
00054 "Goal didn't finish")
00055 self.assertEqual(GoalStatus.ABORTED, client.get_state())
00056
00057 goal = TestGoal(3)
00058 client.send_goal(goal)
00059 self.assert_(client.wait_for_result(rospy.Duration(10.0)),
00060 "Goal didn't finish")
00061
00062
00063 self.assertEqual(GoalStatus.ABORTED, client.get_state())
00064
00065
00066 goal = TestGoal(9)
00067 saved_feedback={};
00068 def on_feedback(fb):
00069 rospy.loginfo("Got feedback")
00070 saved_feedback[0]=fb
00071
00072 client.send_goal(goal,feedback_cb=on_feedback)
00073 self.assert_(client.wait_for_result(rospy.Duration(10.0)),
00074 "Goal didn't finish")
00075 self.assertEqual(GoalStatus.SUCCEEDED, client.get_state())
00076
00077 self.assertEqual(saved_feedback[0].feedback,9)
00078
00079
00080
00081
00082 if __name__ == '__main__':
00083 import rostest
00084 rospy.init_node('test_ref_simple_action_server')
00085 rostest.rosrun('actionlib', 'test_simple_action_client_python', TestRefSimpleActionServer)