Updated RT goal handle to handle cancel requests

ros-controls · Oct 28, 2017 · cd6947e · cd6947e
1 parent 059bcc9
commit cd6947e
Showing 1 changed file with 23 additions and 4 deletions.
diff --git a/include/realtime_tools/realtime_server_goal_handle.h b/include/realtime_tools/realtime_server_goal_handle.h
@@ -55,6 +55,7 @@ class RealtimeServerGoalHandle
   uint8_t state_;
 
   bool req_abort_;
+  bool req_cancel_;
   bool req_succeed_;
   ResultConstPtr req_result_;
   FeedbackConstPtr req_feedback_;
@@ -79,16 +80,25 @@ class RealtimeServerGoalHandle
 
   void setAborted(ResultConstPtr result = ResultConstPtr((Result*)NULL))
   {
-    if (!req_succeed_ && !req_abort_)
+    if (!req_succeed_ && !req_abort_ && !req_cancel_)
     {
       req_result_ = result;
       req_abort_ = true;
     }
   }
 
+  void setCanceled(ResultConstPtr result = ResultConstPtr((Result*)NULL))
+  {
+    if (!req_succeed_ && !req_abort_ && !req_cancel_)
+    {
+      req_result_ = result;
+      req_cancel_ = true;
+    }
+  }
+
   void setSucceeded(ResultConstPtr result = ResultConstPtr((Result*)NULL))
   {
-    if (!req_succeed_ && !req_abort_)
+    if (!req_succeed_ && !req_abort_ && !req_cancel_)
     {
       req_result_ = result;
       req_succeed_ = true;
@@ -111,14 +121,23 @@ class RealtimeServerGoalHandle
     if (valid())
     {
       actionlib_msgs::GoalStatus gs = gh_.getGoalStatus();
-      if (req_abort_ && gs.status == GoalStatus::ACTIVE)
+      if (req_abort_ && (gs.status == GoalStatus::ACTIVE ||
+                         gs.status == GoalStatus::PREEMPTING))
       {
         if (req_result_)
           gh_.setAborted(*req_result_);
         else
           gh_.setAborted();
       }
-      else if (req_succeed_ && gs.status == GoalStatus::ACTIVE)
+      else if (req_cancel_ && gs.status == GoalStatus::PREEMPTING)
+      {
+        if (req_result_)
+          gh_.setCanceled(*req_result_);
+        else
+          gh_.setCanceled();
+      }
+      else if (req_succeed_ && (gs.status == GoalStatus::ACTIVE ||
+                                gs.status == GoalStatus::PREEMPTING))
       {
         if (req_result_)
           gh_.setSucceeded(*req_result_);