From 122355704b8a76af398a0c8049a6656db34c055d Mon Sep 17 00:00:00 2001
From: Kaven Yau <kavenyau@foxmail.com>
Date: Thu, 20 May 2021 06:22:41 +0800
Subject: [PATCH] Returns CancelResponse::REJECT while goal handle failed to
 transit to CANCELING state (#1641) (#1659)

Signed-off-by: Kaven Yau <kavenyau@foxmail.com>
---
 rclcpp_action/include/rclcpp_action/server.hpp | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp
index a6059f1..cde659c 100644
--- a/rclcpp_action/include/rclcpp_action/server.hpp
+++ b/rclcpp_action/include/rclcpp_action/server.hpp
@@ -365,7 +365,14 @@ protected:
     if (goal_handle) {
       resp = handle_cancel_(goal_handle);
       if (CancelResponse::ACCEPT == resp) {
-        goal_handle->_cancel_goal();
+        try {
+          goal_handle->_cancel_goal();
+        } catch (const rclcpp::exceptions::RCLError & ex) {
+          RCLCPP_DEBUG(
+            rclcpp::get_logger("rclcpp_action"),
+            "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what());
+          return CancelResponse::REJECT;
+        }
       }
     }
     return resp;