From c7b394a061a99b2f8ba7c8438e3dd39b488ddc6f Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Fri, 18 Aug 2023 09:14:25 -0700 Subject: [PATCH] rcl_send_response returns RCL_RET_TIMEOUT. (#1048) Signed-off-by: Tomoya Fujita --- rcl/include/rcl/service.h | 1 + rcl/src/rcl/service.c | 11 +++++++---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/rcl/include/rcl/service.h b/rcl/include/rcl/service.h index 56e26459c..330cf17af 100644 --- 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 index 1cca57acc..121e40095 100644 --- 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,