From 9af21b78461611d20184d1903b88e162ca5d0f28 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sat, 1 Oct 2022 11:31:32 +0900 Subject: [PATCH] [rclcpp] add retry for send_response in service.hpp Signed-off-by: Takamasa Horibe --- rclcpp/include/rclcpp/service.hpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 925c5f6286..9b64432487 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -495,7 +495,18 @@ class Service return; } if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response"); + constexpr size_t retry_num = 100; + rcl_ret_t ret_2 = RCL_RET_NOT_INIT; + for (size_t i = 1; i < retry_num; ++i) { + rclcpp::sleep_for(std::chrono::milliseconds(100)); + ret_2 = rcl_send_response(get_service_handle().get(), &req_id, &response); + if (ret_2 == RCL_RET_OK) { + break; + } + } + if (ret_2 != RCL_RET_OK) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to send response"); + } } }