diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index c83ed5c89..e76489823 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -21,6 +21,9 @@ #include "rmw/allocators.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rmw/validate_full_topic_name.h" + +#include "rcpputils/scope_exit.hpp" #include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" @@ -49,22 +52,30 @@ rmw_fastrtps_cpp::create_publisher( bool keyed, bool create_publisher_listener) { - if (!participant_info) { - RMW_SET_ERROR_MSG("participant_info is null"); - return nullptr; - } - if (!topic_name || strlen(topic_name) == 0) { - RMW_SET_ERROR_MSG("publisher topic is null or empty string"); + RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); + if (0 == strlen(topic_name)) { + RMW_SET_ERROR_MSG("topic_name argument is an empty string"); return nullptr; } - if (!qos_policies) { - RMW_SET_ERROR_MSG("qos_policies is null"); - return nullptr; - } - if (!publisher_options) { - RMW_SET_ERROR_MSG("publisher_options is null"); - return nullptr; + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + if (!qos_policies->avoid_ros_namespace_conventions) { + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; + } + if (RMW_TOPIC_VALID != validation_result) { + const char * reason = rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid topic name: %s", reason); + return nullptr; + } } + RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); + + Participant * participant = participant_info->participant; + RMW_CHECK_ARGUMENT_FOR_NULL(participant, nullptr); const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); @@ -93,7 +104,14 @@ rmw_fastrtps_cpp::create_publisher( RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); return nullptr; } - + auto cleanup_info = rcpputils::make_scope_exit( + [info, participant]() { + if (info->type_support_) { + _unregister_type(participant, info->type_support_); + } + delete info->listener_; + delete info; + }); info->typesupport_identifier_ = type_support->typesupport_identifier; info->type_support_impl_ = type_support->data; @@ -101,15 +119,15 @@ rmw_fastrtps_cpp::create_publisher( std::string type_name = _create_type_name(callbacks); if ( !Domain::getRegisteredType( - participant_info->participant, type_name.c_str(), + participant, type_name.c_str(), reinterpret_cast(&info->type_support_))) { info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); if (!info->type_support_) { - RMW_SET_ERROR_MSG("Failed to allocate MessageTypeSupport"); - goto fail; + RMW_SET_ERROR_MSG("failed to allocate MessageTypeSupport"); + return nullptr; } - _register_type(participant_info->participant, info->type_support_); + _register_type(participant, info->type_support_); } if (!participant_info->leave_middleware_default_qos) { @@ -124,8 +142,7 @@ rmw_fastrtps_cpp::create_publisher( publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); if (!get_datawriter_qos(*qos_policies, publisherParam)) { - RMW_SET_ERROR_MSG("failed to get datawriter qos"); - goto fail; + return nullptr; } info->listener_ = nullptr; @@ -133,17 +150,17 @@ rmw_fastrtps_cpp::create_publisher( info->listener_ = new (std::nothrow) PubListener(info); if (!info->listener_) { RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); - goto fail; + return nullptr; } } info->publisher_ = Domain::createPublisher( - participant_info->participant, + participant, publisherParam, info->listener_); if (!info->publisher_) { RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); - goto fail; + return nullptr; } info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( @@ -152,30 +169,27 @@ rmw_fastrtps_cpp::create_publisher( rmw_publisher = rmw_publisher_allocate(); if (!rmw_publisher) { RMW_SET_ERROR_MSG("failed to allocate publisher"); - goto fail; + return nullptr; } + auto cleanup_publisher = rcpputils::make_scope_exit( + [rmw_publisher]() { + rmw_free(const_cast(rmw_publisher->topic_name)); + rmw_publisher_free(rmw_publisher); + }); + rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; rmw_publisher->data = info; - rmw_publisher->topic_name = static_cast(rmw_allocate(strlen(topic_name) + 1)); + rmw_publisher->topic_name = static_cast(rmw_allocate(strlen(topic_name) + 1)); if (!rmw_publisher->topic_name) { RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); - goto fail; + return nullptr; } - memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); rmw_publisher->options = *publisher_options; + cleanup_publisher.cancel(); + cleanup_info.cancel(); return rmw_publisher; - -fail: - if (info) { - delete info->type_support_; - delete info->listener_; - delete info; - } - rmw_publisher_free(rmw_publisher); - - return nullptr; } diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index 98d9079ed..976964f3e 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -19,6 +19,8 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rmw/impl/cpp/macros.hpp" + #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" @@ -63,15 +65,12 @@ rmw_create_publisher( const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options) { - if (!node) { - RMW_SET_ERROR_MSG("node handle is null"); - return nullptr; - } - - if (node->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("node handle not from this implementation"); - return nullptr; - } + RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return nullptr); rmw_publisher_t * publisher = rmw_fastrtps_cpp::create_publisher( static_cast(node->context->impl->participant_info), @@ -101,8 +100,18 @@ rmw_create_publisher( static_cast(&msg), nullptr); if (RMW_RET_OK != rmw_ret) { - rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( + rmw_error_state_t error_state = *rmw_get_error_state(); + rmw_reset_error(); + static_cast(common_context->graph_cache.dissociate_writer( + info->publisher_gid, common_context->gid, node->name, node->namespace_)); + rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( eprosima_fastrtps_identifier, node, publisher); + if (RMW_RET_OK != rmw_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "' cleanup\n"); + rmw_reset_error(); + } + rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); return nullptr; } } @@ -164,6 +173,19 @@ rmw_return_loaned_message_from_publisher( rmw_ret_t rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) { + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, + publisher->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + return rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( eprosima_fastrtps_identifier, node, publisher); } diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 6e5a6ad2c..8a602b613 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -18,6 +18,9 @@ #include "rmw/allocators.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rmw/validate_full_topic_name.h" + +#include "rcpputils/scope_exit.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" @@ -51,31 +54,30 @@ rmw_fastrtps_dynamic_cpp::create_publisher( (void)keyed; (void)create_publisher_listener; - if (!participant_info) { - RMW_SET_ERROR_MSG("participant_info is null"); - return nullptr; - } - - if (!topic_name || strlen(topic_name) == 0) { - RMW_SET_ERROR_MSG("publisher topic is null or empty string"); - return nullptr; - } - - if (!qos_policies) { - RMW_SET_ERROR_MSG("qos_policies is null"); + RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); + if (0 == strlen(topic_name)) { + RMW_SET_ERROR_MSG("topic_name argument is an empty string"); return nullptr; } - - if (!publisher_options) { - RMW_SET_ERROR_MSG("publisher_options is null"); - return nullptr; + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + if (!qos_policies->avoid_ros_namespace_conventions) { + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; + } + if (RMW_TOPIC_VALID != validation_result) { + const char * reason = rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid topic name: %s", reason); + return nullptr; + } } + RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); Participant * participant = participant_info->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } + RMW_CHECK_ARGUMENT_FOR_NULL(participant, nullptr); const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); @@ -104,14 +106,25 @@ rmw_fastrtps_dynamic_cpp::create_publisher( RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); return nullptr; } + auto cleanup_info = rcpputils::make_scope_exit( + [info, participant]() { + if (info->type_support_) { + _unregister_type(participant, info->type_support_); + } + delete info->listener_; + delete info; + }); TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); auto type_impl = type_registry.get_message_type_support(type_support); if (!type_impl) { - delete info; RMW_SET_ERROR_MSG("failed to allocate type support"); return nullptr; } + auto return_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { + type_registry.return_message_type_support(type_support); + }); info->typesupport_identifier_ = type_support->typesupport_identifier; info->type_support_impl_ = type_impl; @@ -126,7 +139,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); if (!info->type_support_) { RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); - goto fail; + return nullptr; } _register_type(participant, info->type_support_); } @@ -150,20 +163,19 @@ rmw_fastrtps_dynamic_cpp::create_publisher( // publisherParam.throughputController = throughputController; if (!get_datawriter_qos(*qos_policies, publisherParam)) { - RMW_SET_ERROR_MSG("failed to get datawriter qos"); - goto fail; + return nullptr; } info->listener_ = new (std::nothrow) PubListener(info); if (!info->listener_) { RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); - goto fail; + return nullptr; } info->publisher_ = Domain::createPublisher(participant, publisherParam, info->listener_); if (!info->publisher_) { RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); - goto fail; + return nullptr; } info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( @@ -172,32 +184,29 @@ rmw_fastrtps_dynamic_cpp::create_publisher( rmw_publisher = rmw_publisher_allocate(); if (!rmw_publisher) { RMW_SET_ERROR_MSG("failed to allocate publisher"); - goto fail; + return nullptr; } + auto cleanup_publisher = rcpputils::make_scope_exit( + [rmw_publisher]() { + rmw_free(const_cast(rmw_publisher->topic_name)); + rmw_publisher_free(rmw_publisher); + }); + rmw_publisher->can_loan_messages = false; rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; rmw_publisher->data = info; - rmw_publisher->topic_name = static_cast(rmw_allocate(strlen(topic_name) + 1)); + rmw_publisher->topic_name = static_cast(rmw_allocate(strlen(topic_name) + 1)); if (!rmw_publisher->topic_name) { RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); - goto fail; + return nullptr; } - memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); rmw_publisher->options = *publisher_options; + cleanup_publisher.cancel(); + cleanup_info.cancel(); + return_type_support.cancel(); return rmw_publisher; - -fail: - if (info) { - delete info->type_support_; - delete info->listener_; - delete info; - } - type_registry.return_message_type_support(type_support); - rmw_publisher_free(rmw_publisher); - - return nullptr; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp index 45e2ce135..e9f5fe3f9 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -18,6 +18,8 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" +#include "rmw/impl/cpp/macros.hpp" + #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" @@ -65,15 +67,12 @@ rmw_create_publisher( const rmw_qos_profile_t * qos_policies, const rmw_publisher_options_t * publisher_options) { - if (!node) { - RMW_SET_ERROR_MSG("node handle is null"); - return nullptr; - } - - if (node->implementation_identifier != eprosima_fastrtps_identifier) { - RMW_SET_ERROR_MSG("node handle not from this implementation"); - return nullptr; - } + RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return nullptr); auto impl = static_cast(node->context->impl->participant_info); @@ -105,8 +104,18 @@ rmw_create_publisher( static_cast(&msg), nullptr); if (RMW_RET_OK != rmw_ret) { - rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( + rmw_error_state_t error_state = *rmw_get_error_state(); + rmw_reset_error(); + static_cast(common_context->graph_cache.dissociate_writer( + info->publisher_gid, common_context->gid, node->name, node->namespace_)); + rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( eprosima_fastrtps_identifier, node, publisher); + if (RMW_RET_OK != rmw_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "' cleanup\n"); + rmw_reset_error(); + } + rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); return nullptr; } } @@ -170,12 +179,21 @@ using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; rmw_ret_t rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) { - auto info = static_cast(publisher->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "publisher info pointer is null", return RMW_RET_ERROR); + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, + publisher->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + auto info = static_cast(publisher->data); auto impl = static_cast(info->type_support_impl_); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(impl, "publisher type support is null", return RMW_RET_ERROR); - auto ros_type_support = static_cast( impl->ros_type_support()); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp index 2e9045089..5d626a2ee 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -36,12 +36,12 @@ typedef struct CustomPublisherInfo : public CustomEventInfo { virtual ~CustomPublisherInfo() = default; - eprosima::fastrtps::Publisher * publisher_; - PubListener * listener_; - rmw_fastrtps_shared_cpp::TypeSupport * type_support_; - const void * type_support_impl_; - rmw_gid_t publisher_gid; - const char * typesupport_identifier_; + eprosima::fastrtps::Publisher * publisher_{nullptr}; + PubListener * listener_{nullptr}; + rmw_fastrtps_shared_cpp::TypeSupport * type_support_{nullptr}; + const void * type_support_impl_{nullptr}; + rmw_gid_t publisher_gid{}; + const char * typesupport_identifier_{nullptr}; RMW_FASTRTPS_SHARED_CPP_PUBLIC EventListenerInterface * diff --git a/rmw_fastrtps_shared_cpp/src/publisher.cpp b/rmw_fastrtps_shared_cpp/src/publisher.cpp index 9dc3eb3d7..03c6f5cb0 100644 --- a/rmw_fastrtps_shared_cpp/src/publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/publisher.cpp @@ -35,33 +35,17 @@ destroy_publisher( CustomParticipantInfo * participant_info, rmw_publisher_t * publisher) { - if (!publisher) { - RMW_SET_ERROR_MSG("publisher handle is null"); - return RMW_RET_ERROR; - } - if (publisher->implementation_identifier != identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } - if (!participant_info) { - RMW_SET_ERROR_MSG("participant_info is null"); - return RMW_RET_ERROR; - } + assert(publisher->implementation_identifier == identifier); + static_cast(identifier); auto info = static_cast(publisher->data); - if (info != nullptr) { - if (info->publisher_ != nullptr) { - Domain::removePublisher(info->publisher_); - } - delete info->listener_; - if (info->type_support_ != nullptr) { - Participant * participant = participant_info->participant; - _unregister_type(participant, info->type_support_); - } - delete info; - } + Domain::removePublisher(info->publisher_); + delete info->listener_; + + _unregister_type(participant_info->participant, info->type_support_); + delete info; + rmw_free(const_cast(publisher->topic_name)); - publisher->topic_name = nullptr; rmw_publisher_free(publisher); return RMW_RET_OK; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index 99b45bac1..143c7ced1 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -37,25 +37,11 @@ __rmw_destroy_publisher( const rmw_node_t * node, rmw_publisher_t * publisher) { - if (!node) { - RMW_SET_ERROR_MSG("node handle is null"); - return RMW_RET_ERROR; - } - - if (node->implementation_identifier != identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } - - if (!publisher) { - RMW_SET_ERROR_MSG("publisher handle is null"); - return RMW_RET_ERROR; - } - if (publisher->implementation_identifier != identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } + assert(node->implementation_identifier == identifier); + assert(publisher->implementation_identifier == identifier); + rmw_ret_t ret = RMW_RET_OK; + rmw_error_state_t error_state; auto common_context = static_cast(node->context->impl->common); auto info = static_cast(publisher->data); { @@ -64,22 +50,36 @@ __rmw_destroy_publisher( rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.dissociate_writer( info->publisher_gid, common_context->gid, node->name, node->namespace_); - rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + rmw_ret_t publish_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, common_context->pub, &msg, nullptr); - if (RMW_RET_OK != rmw_ret) { - return rmw_ret; + if (RMW_RET_OK != publish_ret) { + error_state = *rmw_get_error_state(); + ret = publish_ret; + rmw_reset_error(); } } auto participant_info = static_cast(node->context->impl->participant_info); - return destroy_publisher( - identifier, - participant_info, - publisher); + rmw_ret_t inner_ret = destroy_publisher(identifier, participant_info, publisher); + if (RMW_RET_OK != inner_ret) { + if (RMW_RET_OK != ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + } else { + error_state = *rmw_get_error_state(); + ret = inner_ret; + } + rmw_reset_error(); + } + + if (RMW_RET_OK != ret) { + rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); + } + return ret; } rmw_ret_t