try aborting before canceling 1st on dtor of ServerGoalHandle. (#2953)

* try aborting before canceling 1st on dtor of ServerGoalHandle.

Signed-off-by: Tomoya Fujita <[email protected]>

* address review comments from Copilot AI.

Signed-off-by: Tomoya Fujita <[email protected]>

* add missing header file and descriptions in docsection.

Signed-off-by: Tomoya Fujita <[email protected]>

---------

Signed-off-by: Tomoya Fujita <[email protected]>

Author: fujitatomoya

rclcpp_action/include/rclcpp_action/server_goal_handle.hpp

@@ -24,6 +24,8 @@
 
 #include "action_msgs/msg/goal_status.hpp"
 
+#include "rclcpp/logging.hpp"
+
 #include "rclcpp_action/visibility_control.hpp"
 #include "rclcpp_action/types.hpp"
 
@@ -103,11 +105,19 @@ class ServerGoalHandleBase
   _execute();
 
   /// Transition the goal to canceled state if it never reached a terminal state.
+  /// Returns true if transitioned to canceled, else false.
   /// \internal
   RCLCPP_ACTION_PUBLIC
   bool
   try_canceling() noexcept;
 
+  /// Transition the goal to aborted state if it never reached a terminal state.
+  /// Returns true if transitioned to aborted, else false.
+  /// \internal
+  RCLCPP_ACTION_PUBLIC
+  bool
+  try_aborting() noexcept;
+
   // End API for communication between ServerGoalHandleBase and ServerGoalHandle<>
   // -----------------------------------------------------------------------------
 
@@ -243,11 +253,22 @@ class ServerGoalHandle : public ServerGoalHandleBase
 
   virtual ~ServerGoalHandle()
   {
-    // Cancel goal if handle was allowed to destruct without reaching a terminal state
-    if (try_canceling()) {
-      auto null_result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
-      null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
-      on_terminal_state_(uuid_, null_result);
+    try {
+      // Abort goal if handle was allowed to destruct without reaching a terminal state
+      if (try_aborting()) {
+        auto null_result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
+        null_result->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
+        on_terminal_state_(uuid_, null_result);
+      } else if (try_canceling()) {
+        // Cancel goal if handle was allowed to destruct without reaching a terminal state
+        auto null_result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
+        null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
+        on_terminal_state_(uuid_, null_result);
+      }
+    } catch (const std::exception & ex) {
+      RCLCPP_DEBUG(
+        rclcpp::get_logger("rclcpp_action"),
+        "Failed to abort/cancel goal handler in destructor: %s", ex.what());
     }
   }
 

rclcpp_action/src/server_goal_handle.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <memory>
+#include <mutex>
 
 #include "rcl_action/action_server.h"
 #include "rcl_action/goal_handle.h"
@@ -135,6 +136,24 @@ ServerGoalHandleBase::try_canceling() noexcept
     return RCL_RET_OK == ret;
   }
 
-  return false;
+  return is_cancelable;
+}
+
+bool
+ServerGoalHandleBase::try_aborting() noexcept
+{
+  std::lock_guard<std::mutex> lock(rcl_handle_mutex_);
+  rcl_ret_t ret;
+  // Check if the goal is abortable
+  const bool is_abortable = rcl_action_goal_handle_is_abortable(rcl_handle_.get());
+  if (is_abortable) {
+    // Transition to ABORTED
+    ret = rcl_action_update_goal_state(rcl_handle_.get(), GOAL_EVENT_ABORT);
+    if (RCL_RET_OK != ret) {
+      return false;
+    }
+  }
+
+  return is_abortable;
 }
 }  // namespace rclcpp_action

rclcpp_action/test/test_server_goal_handle.cpp

@@ -44,6 +44,8 @@ class FibonacciServerGoalHandle
 
   bool try_cancel() {return try_canceling();}
 
+  bool try_abort() {return try_aborting();}
+
   void cancel_goal() {_cancel_goal();}
 };
 
@@ -132,6 +134,20 @@ TEST_F(TestServerGoalHandle, abort) {
   EXPECT_THROW(handle_->abort(result), rclcpp::exceptions::RCLError);
 }
 
+TEST_F(TestServerGoalHandle, try_abort) {
+  handle_->execute();
+  test_msgs::action::Fibonacci::Result::SharedPtr result =
+    std::make_shared<test_msgs::action::Fibonacci::Result>();
+  EXPECT_TRUE(handle_->try_abort());
+  EXPECT_FALSE(handle_->is_canceling());
+  EXPECT_FALSE(handle_->is_active());
+  EXPECT_FALSE(handle_->is_executing());
+
+  auto mock = mocking_utils::patch_and_return(
+    "lib:rclcpp_action", rcl_action_update_goal_state, RCL_RET_ERROR);
+  EXPECT_THROW(handle_->abort(result), rclcpp::exceptions::RCLError);
+}
+
 TEST_F(TestServerGoalHandle, succeed) {
   handle_->execute();
   test_msgs::action::Fibonacci::Result::SharedPtr result =