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/simple_action_server.h>
00038 #include <actionlib/TestAction.h>
00039 #include <ros/ros.h>
00040
00041 namespace actionlib
00042 {
00043
00044 class SimpleExecuteRefServer
00045 {
00046 public:
00047 typedef ServerGoalHandle<TestAction> GoalHandle;
00048
00049 SimpleExecuteRefServer();
00050
00051 private:
00052 ros::NodeHandle nh_;
00053 SimpleActionServer<TestAction> as_;
00054
00055 void executeCallback(const TestGoalConstPtr& goal);
00056 };
00057
00058 }
00059
00060 using namespace actionlib;
00061
00062 SimpleExecuteRefServer::SimpleExecuteRefServer() : as_(nh_, "reference_action", boost::bind(&SimpleExecuteRefServer::executeCallback, this, _1), false)
00063 {
00064 as_.start();
00065 }
00066
00067 void SimpleExecuteRefServer::executeCallback(const TestGoalConstPtr& goal)
00068 {
00069 ROS_DEBUG_NAMED("actionlib", "Got a goal of type [%u]", goal->goal);
00070 switch (goal->goal)
00071 {
00072 case 1:
00073 ROS_DEBUG_NAMED("actionlib", "Got goal #1");
00074 as_.setSucceeded(TestResult(), "The ref server has succeeded");
00075 break;
00076 case 2:
00077 ROS_DEBUG_NAMED("actionlib", "Got goal #2");
00078 as_.setAborted(TestResult(), "The ref server has aborted");
00079 break;
00080 case 4:
00081 {
00082 ROS_DEBUG_NAMED("actionlib", "Got goal #4");
00083 ros::Duration sleep_dur(.1);
00084 for (unsigned int i=0; i<100; i++)
00085 {
00086 sleep_dur.sleep();
00087 if (as_.isPreemptRequested())
00088 {
00089 as_.setPreempted();
00090 return;
00091 }
00092 }
00093 as_.setAborted();
00094 break;
00095 }
00096 default:
00097 break;
00098 }
00099 }
00100
00101 int main(int argc, char** argv)
00102 {
00103 ros::init(argc, argv, "ref_server");
00104
00105 SimpleExecuteRefServer server;
00106
00107 ros::spin();
00108
00109 return 0;
00110 }
00111
00112