rcl_send_response returns RCL_RET_TIMEOUT. (#1048)

Signed-off-by: Tomoya Fujita <[email protected]>
ros2 · Aug 18, 2023 · c7b394a · c7b394a
1 parent 799151f
commit c7b394a
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 4 deletions.
diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h
@@ -330,6 +330,7 @@ rcl_take_request(
  * \return #RCL_RET_OK if the response was sent successfully, or
  * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or
  * \return #RCL_RET_SERVICE_INVALID if the service is invalid, or
+ * \return #RCL_RET_TIMEOUT if a response reader is not ready yet, or
  * \return #RCL_RET_ERROR if an unspecified error occurs.
  */
 RCL_PUBLIC

diff --git a/rcl/src/rcl/service.c b/rcl/src/rcl/service.c
@@ -387,6 +387,7 @@ rcl_send_response(
   rmw_request_id_t * request_header,
   void * ros_response)
 {
+  rcl_ret_t ret;
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending service response");
   if (!rcl_service_is_valid(service)) {
     return RCL_RET_SERVICE_INVALID;  // error already set
@@ -396,16 +397,18 @@ rcl_send_response(
   const rcl_service_options_t * options = rcl_service_get_options(service);
   RCL_CHECK_FOR_NULL_WITH_MSG(options, "Failed to get service options", return RCL_RET_ERROR);
 
-  if (rmw_send_response(
-      service->impl->rmw_handle, request_header, ros_response) != RMW_RET_OK)
-  {
+  ret = rmw_send_response(service->impl->rmw_handle, request_header, ros_response);
+  if (ret != RMW_RET_OK) {
     RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+    if (ret == RMW_RET_TIMEOUT) {
+      return RCL_RET_TIMEOUT;
+    }
     return RCL_RET_ERROR;
   }
 
   // publish out the introspected content
   if (service->impl->service_event_publisher != NULL) {
-    rcl_ret_t ret = rcl_send_service_event_message(
+    ret = rcl_send_service_event_message(
       service->impl->service_event_publisher,
       service_msgs__msg__ServiceEventInfo__RESPONSE_SENT,
       ros_response,