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
00030
00031
00032 #include <gtest/gtest.h>
00033 #include <ros/ros.h>
00034 #include <actionlib/TestRequestAction.h>
00035 #include <actionlib/TestRequestGoal.h>
00036 #include <actionlib/client/simple_action_client.h>
00037 #include <actionlib_msgs/GoalStatus.h>
00038
00039
00040 #define EXPECT_STATE(expected_state) EXPECT_TRUE( \
00041 ac_.getState() == SimpleClientGoalState::expected_state) \
00042 << "Expected [" << #expected_state << "], but goal state is [" << ac_.getState().toString() << \
00043 "]";
00044
00045
00046 using namespace actionlib;
00047 using namespace actionlib_msgs;
00048
00049 class SimpleClientFixture : public testing::Test
00050 {
00051 public:
00052 SimpleClientFixture()
00053 : ac_("test_request_action"), default_wait_(60.0) {}
00054
00055 protected:
00056 virtual void SetUp()
00057 {
00058
00059 ASSERT_TRUE(ac_.waitForServer(ros::Duration(10.0)) );
00060 }
00061
00062 SimpleActionClient<TestRequestAction> ac_;
00063 ros::Duration default_wait_;
00064 };
00065
00066 TEST_F(SimpleClientFixture, just_succeed) {
00067 TestRequestGoal goal;
00068 goal.terminate_status = TestRequestGoal::TERMINATE_SUCCESS;
00069 goal.the_result = 42;
00070 ac_.sendGoal(goal);
00071 ac_.waitForResult(default_wait_);
00072 EXPECT_STATE(SUCCEEDED);
00073 EXPECT_EQ(42, ac_.getResult()->the_result);
00074 }
00075
00076 TEST_F(SimpleClientFixture, just_abort) {
00077 TestRequestGoal goal;
00078 goal.terminate_status = TestRequestGoal::TERMINATE_ABORTED;
00079 goal.the_result = 42;
00080 ac_.sendGoal(goal);
00081 ac_.waitForResult(default_wait_);
00082 EXPECT_STATE(ABORTED);
00083 EXPECT_EQ(42, ac_.getResult()->the_result);
00084 }
00085
00086 TEST_F(SimpleClientFixture, just_preempt) {
00087 TestRequestGoal goal;
00088 goal.terminate_status = TestRequestGoal::TERMINATE_SUCCESS;
00089 goal.delay_terminate = ros::Duration(1000);
00090 goal.the_result = 42;
00091 ac_.sendGoal(goal);
00092
00093
00094 for (unsigned int i = 0; i < 100; i++) {
00095 ROS_DEBUG_NAMED("actionlib", "Waiting for Server Response");
00096 if (ac_.getState() != SimpleClientGoalState::PENDING) {
00097 break;
00098 }
00099 ros::Duration(0.1).sleep();
00100 }
00101
00102 ac_.cancelGoal();
00103 ac_.waitForResult(default_wait_);
00104 EXPECT_STATE(PREEMPTED);
00105 EXPECT_EQ(42, ac_.getResult()->the_result);
00106 }
00107
00108 TEST_F(SimpleClientFixture, drop) {
00109 TestRequestGoal goal;
00110 goal.terminate_status = TestRequestGoal::TERMINATE_DROP;
00111 goal.the_result = 42;
00112 ac_.sendGoal(goal);
00113 ac_.waitForResult(default_wait_);
00114 EXPECT_STATE(ABORTED);
00115 EXPECT_EQ(0, ac_.getResult()->the_result);
00116 }
00117
00118 TEST_F(SimpleClientFixture, exception) {
00119 TestRequestGoal goal;
00120 goal.terminate_status = TestRequestGoal::TERMINATE_EXCEPTION;
00121 goal.the_result = 42;
00122 ac_.sendGoal(goal);
00123 ac_.waitForResult(default_wait_);
00124 EXPECT_STATE(ABORTED);
00125 EXPECT_EQ(0, ac_.getResult()->the_result);
00126 }
00127
00128 TEST_F(SimpleClientFixture, ignore_cancel_and_succeed) {
00129 TestRequestGoal goal;
00130 goal.terminate_status = TestRequestGoal::TERMINATE_SUCCESS;
00131 goal.delay_terminate = ros::Duration(2.0);
00132 goal.ignore_cancel = true;
00133 goal.the_result = 42;
00134 ac_.sendGoal(goal);
00135
00136
00137 for (unsigned int i = 0; i < 100; i++) {
00138 ROS_DEBUG_NAMED("actionlib", "Waiting for Server Response");
00139 if (ac_.getState() != SimpleClientGoalState::PENDING) {
00140 break;
00141 }
00142 ros::Duration(0.1).sleep();
00143 }
00144
00145 ac_.cancelGoal();
00146 ac_.waitForResult(default_wait_ + default_wait_);
00147 EXPECT_STATE(SUCCEEDED);
00148 EXPECT_EQ(42, ac_.getResult()->the_result);
00149 }
00150
00151 TEST_F(SimpleClientFixture, lose) {
00152 TestRequestGoal goal;
00153 goal.terminate_status = TestRequestGoal::TERMINATE_LOSE;
00154 goal.the_result = 42;
00155 ac_.sendGoal(goal);
00156 ac_.waitForResult(default_wait_);
00157 EXPECT_STATE(LOST);
00158 }
00159
00160 void spinThread()
00161 {
00162 ros::NodeHandle nh;
00163 ros::spin();
00164 }
00165
00166 int main(int argc, char ** argv)
00167 {
00168 testing::InitGoogleTest(&argc, argv);
00169
00170 ros::init(argc, argv, "simple_client_test");
00171
00172 boost::thread spin_thread(&spinThread);
00173
00174 int result = RUN_ALL_TESTS();
00175
00176 ros::shutdown();
00177
00178 spin_thread.join();
00179
00180 return result;
00181 }