exercise_simple_client.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # Copyright (c) 2009, Willow Garage, Inc.
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the Willow Garage, Inc. nor the names of its
00014 #       contributors may be used to endorse or promote products derived from
00015 #       this software without specific prior written permission.
00016 #
00017 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027 # POSSIBILITY OF SUCH DAMAGE.
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 GoalStatus
00037 from actionlib.msg import TestRequestAction, TestRequestGoal
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         # Ensure that the action server got the goal, before continuing
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     # Should print out errors about not setting a terminal status in the action server.
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     # Should print out errors about throwing an exception
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         # Ensure that the action server got the goal, before continuing
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     def test_lose(self):
00122         goal = TestRequestGoal(terminate_status=TestRequestGoal.TERMINATE_LOSE,
00123                                the_result=42)
00124         self.client.send_goal(goal)
00125         self.client.wait_for_result(self.default_wait)
00126 
00127         self.assertEqual(GoalStatus.LOST, self.client.get_state())
00128 
00129     # test_freeze_server has been removed, as it is undecided what should happen
00130     # when the action server disappears.
00131     #
00132     # # def test_freeze_server(self):
00133     # #     goal = TestRequestGoal(terminate_status = TestRequestGoal.TERMINATE_SUCCESS,
00134     # #                            the_result = 42,
00135     # #                            pause_status = rospy.Duration(10.0))
00136     # #     self.client.send_goal(goal)
00137     # #     self.client.wait_for_result(rospy.Duration(13.0))
00138     # #
00139     # #     self.assertEqual(GoalStatus.LOST, self.client.get_state())
00140     #
00141 
00142 
00143 if __name__ == '__main__':
00144     rospy.init_node("exercise_simple_server")
00145     rostest.run(PKG, 'exercise_simple_server', SimpleExerciser, sys.argv)


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep
autogenerated on Thu Sep 28 2017 02:51:16