00001 #ifndef CONVENIENCE_ROS_FUNCTIONS_ACTIONSERVER_H 00002 #define CONVENIENCE_ROS_FUNCTIONS_ACTIONSERVER_H 00003 00004 #include <baselib_binding/Thread.h> 00005 #include <baselib_binding/SharedPtr.h> 00006 #include <actionlib/server/action_server.h> 00007 #include <convenience_ros_functions/ROSFunctions.h> 00008 00009 namespace convenience_ros_functions 00010 { 00011 00023 template<class ActionMessage> 00024 class ActionServer 00025 { 00026 ACTION_DEFINITION(ActionMessage) 00027 protected: 00028 typedef ActionServer<ActionMessage> Self; 00029 00030 typedef actionlib::ActionServer<ActionMessage> ROSActionServerT; 00031 // typedef typename baselib_binding::shared_ptr<ROSActionServerT> ROSActionServerPtr; 00032 typedef ROSActionServerT * ROSActionServerPtr; 00033 typedef typename ROSActionServerT::GoalHandle ActionGoalHandleT; 00034 00035 typedef Goal GoalT; 00036 typedef GoalConstPtr GoalConstPtrT; 00037 typedef ActionGoal ActionGoalT; 00038 typedef ActionGoalPtr ActionGoalPtrT; 00039 typedef ActionGoalConstPtr ActionGoalConstPtrT; 00040 typedef ActionResult ActionResultT; 00041 typedef ActionResultConstPtr ActionResultConstPtrT; 00042 typedef Result ResultT; 00043 typedef ResultConstPtr ResultConstPtrT; 00044 typedef Feedback FeedbackT; 00045 typedef FeedbackConstPtr FeedbackConstPtrT; 00046 typedef ActionFeedback ActionFeedbackT; 00047 typedef ActionFeedbackConstPtr ActionFeedbackConstPtrT; 00048 public: 00049 00050 ActionServer<ActionMessage>( 00051 ros::NodeHandle& _node, 00052 const std::string& action_topic); 00053 virtual ~ActionServer<ActionMessage>(); 00054 00055 00059 bool init(); 00060 00064 void shutdown(); 00065 00069 bool executingGoal(); 00070 00071 protected: 00072 00079 virtual float waitForExecution(float timeout); 00080 00084 double timeRunning(); 00085 00091 void currentActionDone(ResultT& result, const actionlib::SimpleClientGoalState& state); 00092 00097 void currentActionDone(const actionlib::SimpleClientGoalState& state); 00098 00108 void currentActionSuccess(const bool success); 00109 00113 virtual bool initImpl(); 00114 00118 virtual void shutdownImpl() {} 00119 00126 virtual bool canAccept(const ActionGoalHandleT& goal)=0; 00127 00137 virtual void actionCallbackImpl(const ActionGoalHandleT& goal)=0; 00138 00144 virtual void actionCancelCallbackImpl(const ActionGoalHandleT& goal) {} 00145 00146 private: 00147 00148 void startServer(); 00149 00150 void deleteServer(); 00151 00152 #if ROS_VERSION_MINIMUM(1, 12, 0) 00153 00156 void actionCallback(ActionGoalHandleT goal); 00157 00161 void actionCancelCallback(ActionGoalHandleT goal); 00162 #else 00163 00166 void actionCallback(ActionGoalHandleT& goal); 00167 00171 void actionCancelCallback(ActionGoalHandleT& goal); 00172 #endif 00173 00174 bool hasCurrentGoal(); 00175 00176 typedef baselib_binding::recursive_mutex recursive_mutex; 00177 typedef baselib_binding::mutex mutex; 00178 typedef baselib_binding::unique_lock<mutex>::type unique_lock; 00179 typedef baselib_binding::unique_lock<recursive_mutex>::type unique_recursive_lock; 00180 typedef baselib_binding::condition_variable condition_variable; 00181 00182 00183 ros::NodeHandle node; 00184 00185 std::string actionTopic; 00186 00187 bool initialized; 00188 bool hasGoal; 00189 bool lastExeSuccess; 00190 ActionGoalHandleT currentGoal; 00191 mutex goalLock; //to lock access to currentGoal 00192 00193 ROSActionServerPtr actionServer; 00194 ros::Time startTime, endTime; 00195 condition_variable executionFinishedCondition; 00196 mutex executionFinishedMutex; 00197 }; 00198 00199 #include <convenience_ros_functions/ActionServer.hpp> 00200 00201 } // namespace 00202 #endif // CONVENIENCE_ROS_FUNCTIONS_ACTIONSERVER_H