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
00035 #ifndef BWI_INTERRUPTABLE_ACTION_SERVER_H_
00036 #define BWI_INTERRUPTABLE_ACTION_SERVER_H_
00037
00038 #include <boost/thread/condition.hpp>
00039 #include <ros/ros.h>
00040 #include <actionlib/server/action_server.h>
00041 #include <actionlib/client/simple_action_client.h>
00042 #include <actionlib/action_definition.h>
00043 #include <std_srvs/Empty.h>
00044
00045 namespace bwi_interruptable_action_server {
00046 template <class ActionSpec>
00047 class InterruptableActionServer {
00048 public:
00049
00050 ACTION_DEFINITION(ActionSpec);
00051
00052 typedef typename actionlib::ActionServer<ActionSpec>::GoalHandle GoalHandle;
00053 typedef boost::function<void(const ResultConstPtr&, const actionlib::SimpleClientGoalState&, int)> ResultCallback;
00054 typedef boost::function<void(const GoalConstPtr&)> NewGoalCallback;
00055
00056 InterruptableActionServer(ros::NodeHandle n,
00057 std::string name,
00058 int max_attempts = 1,
00059 NewGoalCallback new_goal_callback = 0,
00060 ResultCallback result_callback = 0);
00061 ~InterruptableActionServer();
00062
00063 void spin();
00064
00065 protected:
00066
00067 bool pause(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
00068 bool resume(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
00069 void goalCallback(GoalHandle goal);
00070 void cancelCallback(GoalHandle preempt);
00071
00072 void publishFeedback(const FeedbackConstPtr& feedback);
00073
00074 ros::NodeHandle n_;
00075
00076 ros::ServiceServer pause_server_;
00077 ros::ServiceServer resume_server_;
00078
00079 boost::shared_ptr<actionlib::ActionServer<ActionSpec> > as_;
00080 boost::shared_ptr<actionlib::SimpleActionClient<ActionSpec> > ac_;
00081
00082 boost::recursive_mutex lock_;
00083
00084 std::string interruptable_server_name_;
00085
00086 GoalHandle current_goal_, original_goal_, next_goal_;
00087
00088 bool original_goal_available_;
00089 bool switch_to_original_goal_;
00090 bool next_goal_available_;
00091 bool pursue_current_goal_;
00092 bool pursuing_current_goal_;
00093
00094 int max_attempts_;
00095 int current_attempts_;
00096 ResultCallback result_callback_;
00097
00098 NewGoalCallback new_goal_callback_;
00099 };
00100 };
00101
00102
00103 #include <bwi_interruptable_action_server/interruptable_action_server_imp.h>
00104 #endif