update RCL_RET_TIMEOUT error handling with action service response. (#…

…1258) * update RCL_RET_TIMEOUT error handling with action service response. Signed-off-by: Tomoya Fujita <[email protected]> * address review comment. Signed-off-by: Tomoya Fujita <[email protected]> --------- Signed-off-by: Tomoya Fujita <[email protected]>
ros2 · Mar 27, 2024 · 2ec6797 · 2ec6797
1 parent a3ecd7e
commit 2ec6797
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 2 deletions.
diff --git a/rclpy/rclpy/action/server.py b/rclpy/rclpy/action/server.py
@@ -387,7 +387,12 @@ async def _execute_cancel_request(self, request_header_and_message):
                 # Remove from response
                 cancel_response.goals_canceling.remove(goal_info)
 
-        self._handle.send_cancel_response(request_header, cancel_response)
+        try:
+            # If the client goes away anytime before this, sending the goal response may fail.
+            # Catch the exception here and go on so we don't crash.
+            self._handle.send_cancel_response(request_header, cancel_response)
+        except RCLError:
+            self._logger.warn('Failed to send cancel response (the client may have gone away)')
 
     async def _execute_get_result_request(self, request_header_and_message):
         request_header, result_request = request_header_and_message

diff --git a/rclpy/src/rclpy/action_server.cpp b/rclpy/src/rclpy/action_server.cpp
@@ -140,7 +140,15 @@ ActionServer::take_result_request(py::object pymsg_type)
   rcl_ret_t ret = rcl_action_send_ ## Type ## _response( \
     rcl_action_server_.get(), header, ros_response.get()); \
   if (RCL_RET_OK != ret) { \
-    throw rclpy::RCLError("Failed to send " #Type " response"); \
+    if (RCL_RET_TIMEOUT == ret) { \
+      int stack_level = 1; \
+      PyErr_WarnFormat( \
+        PyExc_RuntimeWarning, stack_level, "failed to send response (timeout): %s", \
+        rcl_get_error_string().str); \
+      rcl_reset_error(); \
+    } else { \
+      throw rclpy::RCLError("Failed to send " #Type " response"); \
+    } \
   }
 
 void