From cc6e4e0bac90dfd4ee0f403c5c712e13bbcc25b7 Mon Sep 17 00:00:00 2001
From: Nick Lamprianidis <nlamprian@gmail.com>
Date: Sun, 29 Oct 2017 00:24:52 +0300
Subject: [PATCH] Updated RT goal handle to handle cancel requests

---
 .../realtime_server_goal_handle.h             | 28 ++++++++++++++++---
 1 file changed, 24 insertions(+), 4 deletions(-)

diff --git a/include/realtime_tools/realtime_server_goal_handle.h b/include/realtime_tools/realtime_server_goal_handle.h
index ed1be2af..2f45d147 100644
--- 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_;
@@ -66,6 +67,7 @@ class RealtimeServerGoalHandle
 
   RealtimeServerGoalHandle(GoalHandle &gh, const ResultPtr &preallocated_result = ResultPtr((Result*)NULL), const FeedbackPtr &preallocated_feedback = FeedbackPtr((Feedback*)NULL))
     : req_abort_(false),
+      req_cancel_(false),
       req_succeed_(false),
       gh_(gh),
       preallocated_result_(preallocated_result),
@@ -79,16 +81,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 +122,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_);