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 <ros/ros.h>
00040 #include <cstdio>
00041 #include <string>
00042
00043 namespace actionlib
00044 {
00045
00046 class RefServer : public ActionServer<TestAction>
00047 {
00048 public:
00049 typedef ServerGoalHandle<TestAction> GoalHandle;
00050
00051 RefServer(ros::NodeHandle & n, const std::string & name);
00052
00053 private:
00054 void goalCallback(GoalHandle gh);
00055 void cancelCallback(GoalHandle gh);
00056 };
00057
00058 }
00059
00060 using namespace actionlib;
00061
00062 RefServer::RefServer(ros::NodeHandle & n, const std::string & name)
00063 : ActionServer<TestAction>(n, name,
00064 boost::bind(&RefServer::goalCallback, this, _1),
00065 boost::bind(&RefServer::cancelCallback, this, _1),
00066 false)
00067 {
00068 start();
00069 printf("Creating ActionServer [%s]\n", name.c_str());
00070 }
00071
00072 void RefServer::goalCallback(GoalHandle gh)
00073 {
00074 TestGoal goal = *gh.getGoal();
00075
00076 switch (goal.goal) {
00077 case 1:
00078 gh.setAccepted();
00079 gh.setSucceeded(TestResult(), "The ref server has succeeded");
00080 break;
00081 case 2:
00082 gh.setAccepted();
00083 gh.setAborted(TestResult(), "The ref server has aborted");
00084 break;
00085 case 3:
00086 gh.setRejected(TestResult(), "The ref server has rejected");
00087 break;
00088 default:
00089 break;
00090 }
00091 }
00092
00093 void RefServer::cancelCallback(GoalHandle)
00094 {
00095 }
00096
00097
00098 int main(int argc, char ** argv)
00099 {
00100 ros::init(argc, argv, "ref_server");
00101
00102 ros::NodeHandle nh;
00103
00104 RefServer ref_server(nh, "reference_action");
00105
00106 ros::spin();
00107 }