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
00033
00034
00036
00037 #include <gtest/gtest.h>
00038 #include <actionlib/TestAction.h>
00039 #include <actionlib/client/simple_action_client.h>
00040
00041 using namespace actionlib;
00042
00043 TEST(SimpleClient, easy_tests) {
00044 ros::NodeHandle n;
00045 SimpleActionClient<TestAction> client(n, "reference_action");
00046
00047 bool started = client.waitForServer(ros::Duration(10.0));
00048 ASSERT_TRUE(started);
00049
00050 TestGoal goal;
00051 bool finished;
00052
00053 goal.goal = 1;
00054 client.sendGoal(goal);
00055 finished = client.waitForResult(ros::Duration(10.0));
00056 ASSERT_TRUE(finished);
00057 EXPECT_TRUE( client.getState() == SimpleClientGoalState::SUCCEEDED)
00058 << "Expected [SUCCEEDED], but goal state is [" << client.getState().toString() << "]";
00059
00060
00061 EXPECT_TRUE(client.getState().getText() == "The ref server has succeeded");
00062
00063 goal.goal = 2;
00064 client.sendGoal(goal);
00065 finished = client.waitForResult(ros::Duration(10.0));
00066 ASSERT_TRUE(finished);
00067 EXPECT_TRUE( client.getState() == SimpleClientGoalState::ABORTED)
00068 << "Expected [ABORTED], but goal state is [" << client.getState().toString() << "]";
00069
00070
00071 EXPECT_TRUE(client.getState().getText() == "The ref server has aborted");
00072
00073 client.cancelAllGoals();
00074
00075
00076 client.cancelGoalsAtAndBeforeTime(ros::Time(1.0));
00077 }
00078
00079
00080 void easyDoneCallback(bool * called, const SimpleClientGoalState & state,
00081 const TestResultConstPtr &)
00082 {
00083 *called = true;
00084 EXPECT_TRUE(state == SimpleClientGoalState::SUCCEEDED)
00085 << "Expected [SUCCEEDED], but goal state is [" << state.toString() << "]";
00086 }
00087
00088 void easyOldDoneCallback(bool * called, const TerminalState & terminal_state,
00089 const TestResultConstPtr &)
00090 {
00091 *called = true;
00092 EXPECT_TRUE(terminal_state == TerminalState::SUCCEEDED)
00093 << "Expected [SUCCEEDED], but terminal state is [" << terminal_state.toString() << "]";
00094 }
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117 void spinThread()
00118 {
00119 ros::NodeHandle nh;
00120 ros::spin();
00121 }
00122
00123 int main(int argc, char ** argv)
00124 {
00125 testing::InitGoogleTest(&argc, argv);
00126
00127 ros::init(argc, argv, "simple_client_test");
00128
00129 boost::thread spin_thread(&spinThread);
00130
00131 int result = RUN_ALL_TESTS();
00132
00133 ros::shutdown();
00134
00135 spin_thread.join();
00136
00137 return result;
00138 }