diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 62bf16b4e..b82208a6b 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -262,7 +262,8 @@ rcl_borrow_loaned_message( if (!rcl_publisher_is_valid(publisher)) { return RCL_RET_PUBLISHER_INVALID; // error already set } - return rmw_borrow_loaned_message(publisher->impl->rmw_handle, type_support, ros_message); + return rcl_convert_rmw_ret_to_rcl_ret( + rmw_borrow_loaned_message(publisher->impl->rmw_handle, type_support, ros_message)); } rcl_ret_t @@ -274,7 +275,8 @@ rcl_return_loaned_message_from_publisher( return RCL_RET_PUBLISHER_INVALID; // error already set } RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); - return rmw_return_loaned_message_from_publisher(publisher->impl->rmw_handle, loaned_message); + return rcl_convert_rmw_ret_to_rcl_ret( + rmw_return_loaned_message_from_publisher(publisher->impl->rmw_handle, loaned_message)); } rcl_ret_t diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 2e3b37d38..c6e079c90 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -270,10 +270,7 @@ rcl_take( subscription->impl->rmw_handle, ros_message, &taken, message_info_local, allocation); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - if (RMW_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; + return rcl_convert_rmw_ret_to_rcl_ret(ret); } RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Subscription take succeeded: %s", taken ? "true" : "false"); @@ -352,10 +349,7 @@ rcl_take_serialized_message( subscription->impl->rmw_handle, serialized_message, &taken, message_info_local, allocation); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - if (RMW_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; + return rcl_convert_rmw_ret_to_rcl_ret(ret); } RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Subscription serialized take succeeded: %s", taken ? "true" : "false"); @@ -390,10 +384,7 @@ rcl_take_loaned_message( subscription->impl->rmw_handle, loaned_message, &taken, message_info_local, allocation); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); - if (RMW_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; + return rcl_convert_rmw_ret_to_rcl_ret(ret); } RCUTILS_LOG_DEBUG_NAMED( ROS_PACKAGE_NAME, "Subscription loaned take succeeded: %s", taken ? "true" : "false"); @@ -413,8 +404,9 @@ rcl_return_loaned_message_from_subscription( return RCL_RET_SUBSCRIPTION_INVALID; // error already set } RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); - return rmw_return_loaned_message_from_subscription( - subscription->impl->rmw_handle, loaned_message); + return rcl_convert_rmw_ret_to_rcl_ret( + rmw_return_loaned_message_from_subscription( + subscription->impl->rmw_handle, loaned_message)); } const char *