Do not generate the exception when action service response timeout. (r…

…os2#2464) (ros2#2519) * Do not generate the exception when action service response timeout. Signed-off-by: Tomoya Fujita <[email protected]> * address review comment. Signed-off-by: Tomoya Fujita <[email protected]> --------- Signed-off-by: Tomoya Fujita <[email protected]> (cherry picked from commit 6c7764e) Co-authored-by: Tomoya Fujita <[email protected]>
pixel-robotics · May 3, 2024 · d599f9e · d599f9e
1 parent 2e8a23e
commit d599f9e
Showing 1 changed file with 34 additions and 2 deletions.
diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp
@@ -344,7 +344,16 @@ ServerBase::execute_goal_request_received(std::shared_ptr<void> & data)
   }
 
   if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(ret);
+    if (ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        pimpl_->logger_,
+        "Failed to send goal response %s (timeout): %s",
+        to_string(uuid).c_str(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    } else {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
   }
 
   const auto status = response_pair.first;
@@ -483,6 +492,15 @@ ServerBase::execute_cancel_request_received(std::shared_ptr<void> & data)
       pimpl_->action_server_.get(), &request_header, response.get());
   }
 
+  if (ret == RCL_RET_TIMEOUT) {
+    GoalUUID uuid = request->goal_info.goal_id.uuid;
+    RCLCPP_WARN(
+      pimpl_->logger_,
+      "Failed to send cancel response %s (timeout): %s",
+      to_string(uuid).c_str(), rcl_get_error_string().str);
+    rcl_reset_error();
+    return;
+  }
   if (RCL_RET_OK != ret) {
     rclcpp::exceptions::throw_from_rcl_error(ret);
   }
@@ -538,6 +556,14 @@ ServerBase::execute_result_request_received(std::shared_ptr<void> & data)
     std::lock_guard<std::recursive_mutex> lock(pimpl_->action_server_reentrant_mutex_);
     rcl_ret_t rcl_ret = rcl_action_send_result_response(
       pimpl_->action_server_.get(), &request_header, result_response.get());
+    if (rcl_ret == RCL_RET_TIMEOUT) {
+      RCLCPP_WARN(
+        pimpl_->logger_,
+        "Failed to send result response %s (timeout): %s",
+        to_string(uuid).c_str(), rcl_get_error_string().str);
+      rcl_reset_error();
+      return;
+    }
     if (RCL_RET_OK != rcl_ret) {
       rclcpp::exceptions::throw_from_rcl_error(rcl_ret);
     }
@@ -671,7 +697,13 @@ ServerBase::publish_result(const GoalUUID & uuid, std::shared_ptr<void> result_m
       for (auto & request_header : iter->second) {
         rcl_ret_t ret = rcl_action_send_result_response(
           pimpl_->action_server_.get(), &request_header, result_msg.get());
-        if (RCL_RET_OK != ret) {
+        if (ret == RCL_RET_TIMEOUT) {
+          RCLCPP_WARN(
+            pimpl_->logger_,
+            "Failed to send result response %s (timeout): %s",
+            to_string(uuid).c_str(), rcl_get_error_string().str);
+          rcl_reset_error();
+        } else if (RCL_RET_OK != ret) {
           rclcpp::exceptions::throw_from_rcl_error(ret);
         }
       }