exercise_simple_client.cpp
Go to the documentation of this file.
00001 /*
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 
00030 // Derived from excercise_simple_server.py
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     // Make sure that the server comes up
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   // Sleep for 10 seconds or until we hear back from the action server
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   // Sleep for 10 seconds or until we hear back from the action server
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 }


actionlib
Author(s): Eitan Marder-Eppstein, Vijay Pradeep, Mikael Arguedas
autogenerated on Sat Feb 16 2019 03:21:28