- first working version for iri action server: including stop callback, preempt handling

include/iri_action_server/iri_action_server.h

@@ -47,10 +47,11 @@ class IriActionServer
     
   private:
     boost::function<void (const GoalConstPtr&)> start_action_callback_;
+    boost::function<void ()> stop_action_callback_;
     boost::function<bool ()> is_finished_callback_;
     boost::function<bool ()> has_succeed_callback_;
     boost::function<void (ResultPtr&)> get_result_callback_;
-    boost::function<void (FeedbackPtr&)> publish_feedback_callback_;
+    boost::function<void (FeedbackPtr&)> get_feedback_callback_;
     
     void executeCallback(const GoalConstPtr& goal);
 
@@ -87,13 +88,15 @@ class IriActionServer
     */
     ~IriActionServer(void);
     
+    void start(void);
     void setLoopRate(ros::Rate r);
 
     void registerStartCallback(boost::function<void (const GoalConstPtr&)> cb);
+    void registerStopCallback(boost::function<void ()> cb);
     void registerIsFinishedCallback( boost::function<bool ()> cb);
     void registerHasSucceedCallback( boost::function<bool ()> cb);
     void registerGetResultCallback(boost::function<void (ResultPtr&)> cb);
-    void registerPublishFeedbackCallback(boost::function<void (FeedbackPtr&)> cb);
+    void registerGetFeedbackCallback(boost::function<void (FeedbackPtr&)> cb);
     
 };
 
@@ -103,13 +106,13 @@ IriActionServer<ActionSpec>::IriActionServer(ros::NodeHandle & nh, const std::st
   as_(nh, action_name_, boost::bind(&IriActionServer<ActionSpec>::executeCallback, this, _1), false),
   loop_rate_(10)
 {
-  // launch action server
-  as_.start();
+  ROS_DEBUG("IriActionServer::Constructor");
 }
 
 template <class ActionSpec>
 IriActionServer<ActionSpec>::~IriActionServer(void)
 {
+  ROS_DEBUG("IriActionServer::Destructor");
 }
 
 template <class ActionSpec>
@@ -119,19 +122,30 @@ void IriActionServer<ActionSpec>::setLoopRate(ros::Rate r)
 }
 
 template <class ActionSpec>
-void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
+void IriActionServer<ActionSpec>::start(void)
 {
+  ROS_DEBUG("IriActionServer::Start");
+
   // check if all callbacks have been registered
-  if( !start_action_callback_ || 
-      !is_finished_callback_  || 
-      !has_succeed_callback_  || 
-      !get_result_callback_   || 
-      !publish_feedback_callback_ )
+  if( start_action_callback_ && 
+      stop_action_callback_  &&
+      is_finished_callback_  && 
+      has_succeed_callback_  && 
+      get_result_callback_   && 
+      get_feedback_callback_ )
   {
-    ROS_ERROR("Some Callbacks have not been registered yet!");
-    return;
+    // launch action server
+    as_.start();
   }
+  else
+    ROS_FATAL("Some Callbacks have not been registered yet!");
+}
 
+template <class ActionSpec>
+void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
+{
+  ROS_DEBUG("IriActionServer::executeCallback::start");
+  
   // init action 
   start_action_callback_(goal);
 
@@ -141,30 +155,40 @@ void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
   // while action is active
   while( is_active )
   {
+    ROS_DEBUG("IriActionServer::executeCallback::Active!");
+    
     // check if preempt has been requested
     // and if ROS connection is OK
     if( as_.isPreemptRequested() || !ros::ok() )
     {
+      ROS_DEBUG("IriActionServer::executeCallback::PREEMPTED!");
       is_active = false;
+      
+      // stop action
+      stop_action_callback_();
+      as_.setPreempted();
     }
     // check if action has finished
     else if( is_finished_callback_() )
     {
+      ROS_DEBUG("IriActionServer::executeCallback::FINISH!");
       is_active = false;
 
       // get action result
-      ResultPtr result;
+      ResultPtr result(new Result);
       get_result_callback_(result);
       
       // action has been successfully accomplished
       if( has_succeed_callback_() )
       {
+	ROS_DEBUG("IriActionServer::executeCallback::SUCCEED!");
         // set action as succeeded
         as_.setSucceeded(*result); 
       }
       // action needs to be aborted
       else
       {
+	ROS_DEBUG("IriActionServer::executeCallback::ABORTED!");
         // set action as aborted
         as_.setAborted(*result); 
       }
@@ -172,9 +196,10 @@ void IriActionServer<ActionSpec>::executeCallback(const GoalConstPtr& goal)
     // action has not finished yet
     else
     {
+      ROS_DEBUG("IriActionServer::executeCallback::feedback");
       // get action feedback and publish it
-      FeedbackPtr feedback;
-      publish_feedback_callback_(feedback);
+      FeedbackPtr feedback(new Feedback);
+      get_feedback_callback_(feedback);
       as_.publishFeedback(*feedback); 
     }
     
@@ -189,6 +214,12 @@ void IriActionServer<ActionSpec>::registerStartCallback(boost::function<void (co
   start_action_callback_ = cb;
 }
 
+template <class ActionSpec>
+void IriActionServer<ActionSpec>::registerStopCallback(boost::function<void ()> cb)
+{
+  stop_action_callback_ = cb;
+}
+
 template <class ActionSpec>
 void IriActionServer<ActionSpec>::registerIsFinishedCallback( boost::function<bool ()> cb)
 {
@@ -208,9 +239,9 @@ void IriActionServer<ActionSpec>::registerGetResultCallback(boost::function<void
 }
 
 template <class ActionSpec>
-void IriActionServer<ActionSpec>::registerPublishFeedbackCallback(boost::function<void (FeedbackPtr&)> cb)
+void IriActionServer<ActionSpec>::registerGetFeedbackCallback(boost::function<void (FeedbackPtr&)> cb)
 {
-  publish_feedback_callback_ = cb;
+  get_feedback_callback_ = cb;
 }
 
 #endif