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