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_wait_test) {
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 SimpleClientGoalState state(SimpleClientGoalState::LOST);
00052
00053 goal.goal = 1;
00054 state = client.sendGoalAndWait(goal, ros::Duration(10, 0), ros::Duration(10, 0));
00055 EXPECT_TRUE(state == SimpleClientGoalState::SUCCEEDED) <<
00056 "Expected [SUCCEEDED], but goal state is [" << client.getState().toString() << "]";
00057
00058 goal.goal = 4;
00059 state = client.sendGoalAndWait(goal, ros::Duration(2, 0), ros::Duration(1, 0));
00060 EXPECT_TRUE(state == SimpleClientGoalState::PREEMPTED) <<
00061 "Expected [PREEMPTED], but goal state is [" << client.getState().toString() << "]";
00062 }
00063
00064 void spinThread()
00065 {
00066 ros::NodeHandle nh;
00067 ros::spin();
00068 }
00069
00070 int main(int argc, char ** argv)
00071 {
00072 testing::InitGoogleTest(&argc, argv);
00073
00074 ros::init(argc, argv, "simple_client_test");
00075
00076 boost::thread spin_thread(&spinThread);
00077
00078 int result = RUN_ALL_TESTS();
00079
00080 ros::shutdown();
00081
00082 spin_thread.join();
00083
00084 return result;
00085 }