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]>

Author: fujitatomoya

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

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