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 <actionlib/server/action_server.h>
00038 #include <actionlib/TestAction.h>
00039 #include <actionlib/client/simple_action_client.h>
00040 #include <ros/ros.h>
00041 #include <gtest/gtest.h>
00042
00043 namespace actionlib
00044 {
00045
00046 class ServerGoalHandleDestructionTester
00047 {
00048 public:
00049 typedef ServerGoalHandle<TestAction> GoalHandle;
00050
00051 ServerGoalHandleDestructionTester();
00052
00053 ros::NodeHandle nh_;
00054 ActionServer<TestAction> * as_;
00055 GoalHandle * gh_;
00056
00057 ~ServerGoalHandleDestructionTester();
00058 void goalCallback(GoalHandle gh);
00059 };
00060
00061 }
00062
00063 using namespace actionlib;
00064
00065 ServerGoalHandleDestructionTester::ServerGoalHandleDestructionTester()
00066 {
00067 as_ = new ActionServer<TestAction>(nh_, "reference_action", false);
00068 as_->start();
00069 as_->registerGoalCallback(boost::bind(&ServerGoalHandleDestructionTester::goalCallback, this,
00070 _1));
00071 gh_ = new GoalHandle();
00072 }
00073
00074 ServerGoalHandleDestructionTester::~ServerGoalHandleDestructionTester()
00075 {
00076 delete as_;
00077 gh_->setAccepted();
00078 delete gh_;
00079 }
00080
00081 void ServerGoalHandleDestructionTester::goalCallback(GoalHandle gh)
00082 {
00083 ROS_ERROR_NAMED("actionlib", "In callback");
00084
00085 *gh_ = gh;
00086
00087 TestGoal goal = *gh.getGoal();
00088
00089 switch (goal.goal) {
00090 case 1:
00091 gh.setAccepted();
00092 gh.setSucceeded(TestResult(), "The ref server has succeeded");
00093 break;
00094 case 2:
00095 gh.setAccepted();
00096 gh.setAborted(TestResult(), "The ref server has aborted");
00097 break;
00098 case 3:
00099 gh.setRejected(TestResult(), "The ref server has rejected");
00100 break;
00101 default:
00102 break;
00103 }
00104
00105 ros::shutdown();
00106 }
00107
00108 void spinner()
00109 {
00110 ros::spin();
00111 }
00112
00113 TEST(ServerGoalHandleDestruction, destruction_test) {
00114 boost::thread spin_thread(&spinner);
00115
00116 ServerGoalHandleDestructionTester server;
00117
00118 SimpleActionClient<TestAction> client("reference_action", true);
00119
00120 ROS_ERROR_NAMED("actionlib", "Waiting for server");
00121 client.waitForServer();
00122 ROS_ERROR_NAMED("actionlib", "Done waiting for server");
00123
00124 TestGoal goal;
00125
00126 goal.goal = 1;
00127 client.sendGoal(goal);
00128 ROS_ERROR_NAMED("actionlib", "Sending goal");
00129
00130 spin_thread.join();
00131 }
00132
00133 int main(int argc, char ** argv)
00134 {
00135 testing::InitGoogleTest(&argc, argv);
00136
00137 ros::init(argc, argv, "ref_server");
00138
00139 return RUN_ALL_TESTS();
00140 }