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()
00063 : as_(nh_, "reference_action", boost::bind(&SimpleExecuteRefServer::executeCallback, this,
00064 _1), false)
00065 {
00066 as_.start();
00067 }
00068
00069 void SimpleExecuteRefServer::executeCallback(const TestGoalConstPtr & goal)
00070 {
00071 ROS_DEBUG_NAMED("actionlib", "Got a goal of type [%u]", goal->goal);
00072 switch (goal->goal) {
00073 case 1:
00074 ROS_DEBUG_NAMED("actionlib", "Got goal #1");
00075 as_.setSucceeded(TestResult(), "The ref server has succeeded");
00076 break;
00077 case 2:
00078 ROS_DEBUG_NAMED("actionlib", "Got goal #2");
00079 as_.setAborted(TestResult(), "The ref server has aborted");
00080 break;
00081 case 4:
00082 {
00083 ROS_DEBUG_NAMED("actionlib", "Got goal #4");
00084 ros::Duration sleep_dur(.1);
00085 for (unsigned int i = 0; i < 100; i++) {
00086 sleep_dur.sleep();
00087 if (as_.isPreemptRequested()) {
00088 as_.setPreempted();
00089 return;
00090 }
00091 }
00092 as_.setAborted();
00093 break;
00094 }
00095 default:
00096 break;
00097 }
00098 }
00099
00100 int main(int argc, char ** argv)
00101 {
00102 ros::init(argc, argv, "ref_server");
00103
00104 SimpleExecuteRefServer server;
00105
00106 ros::spin();
00107
00108 return 0;
00109 }