From 285b3d48fceffbba532c451c324635fac095b0ea Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Thu, 6 Aug 2020 10:44:21 -0300 Subject: [PATCH] Ensure compliant subscription API. (#419) Signed-off-by: Michel Hidalgo --- rmw_fastrtps_cpp/src/rmw_subscription.cpp | 51 ++++++-- rmw_fastrtps_cpp/src/subscription.cpp | 102 ++++++++++------ .../src/rmw_subscription.cpp | 55 ++++++--- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 115 ++++++++++-------- .../custom_subscriber_info.hpp | 12 +- .../src/rmw_subscription.cpp | 55 ++++----- rmw_fastrtps_shared_cpp/src/subscription.cpp | 36 ++---- 7 files changed, 245 insertions(+), 181 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp index 738c6493f..712f6f909 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -61,15 +61,12 @@ rmw_create_subscription( const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_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 participant_info = static_cast(node->context->impl->participant_info); @@ -99,8 +96,18 @@ rmw_create_subscription( static_cast(&msg), nullptr); if (RMW_RET_OK != rmw_ret) { - rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + rmw_error_state_t error_state = *rmw_get_error_state(); + rmw_reset_error(); + static_cast(common_context->graph_cache.dissociate_writer( + info->subscription_gid_, common_context->gid, node->name, node->namespace_)); + rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( eprosima_fastrtps_identifier, node, subscription); + 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; } } @@ -121,13 +128,33 @@ rmw_subscription_get_actual_qos( const rmw_subscription_t * subscription, rmw_qos_profile_t * qos) { - return rmw_fastrtps_shared_cpp::__rmw_subscription_get_actual_qos( - subscription, qos); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, + subscription->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); + + return rmw_fastrtps_shared_cpp::__rmw_subscription_get_actual_qos(subscription, qos); } rmw_ret_t rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) { + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, 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( + subscription, + subscription->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + return rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( eprosima_fastrtps_identifier, node, subscription); } diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 6e1122404..c0fc6941e 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.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/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" @@ -55,27 +58,30 @@ create_subscription( bool keyed, bool create_subscription_listener) { - if (!topic_name || strlen(topic_name) == 0) { - RMW_SET_ERROR_MSG("subscription topic is null or empty string"); - return nullptr; - } - if (!qos_policies) { - RMW_SET_ERROR_MSG("qos_policies is null"); - return nullptr; - } - if (!subscription_options) { - RMW_SET_ERROR_MSG("subscription_options 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 (!participant_info) { - RMW_SET_ERROR_MSG("participant_info 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 argument: %s", reason); + return nullptr; + } } + RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); Participant * participant = participant_info->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(participant, "participant handle is null", return nullptr); + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); if (!type_support) { @@ -89,17 +95,31 @@ create_subscription( if (!is_valid_qos(*qos_policies)) { return nullptr; } - CustomSubscriberInfo * info = nullptr; - rmw_subscription_t * rmw_subscription = nullptr; - eprosima::fastrtps::SubscriberAttributes subscriberParam; // Load default XML profile. + eprosima::fastrtps::SubscriberAttributes subscriberParam; Domain::getDefaultSubscriberAttributes(subscriberParam); - info = new (std::nothrow) CustomSubscriberInfo(); + + CustomSubscriberInfo * info = new (std::nothrow) CustomSubscriberInfo(); if (!info) { RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); return nullptr; } + auto cleanup_info = rcpputils::make_scope_exit( + [info, participant]() { + if (info->type_support_) { + _unregister_type(participant, info->type_support_); + } + if (info->subscriber_) { + if (!Domain::removeSubscriber(info->subscriber_)) { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to remove subscriber after '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + } + delete info->listener_; + delete info; + }); info->typesupport_identifier_ = type_support->typesupport_identifier; info->type_support_impl_ = type_support->data; @@ -113,7 +133,7 @@ create_subscription( info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); if (!info->type_support_) { RMW_SET_ERROR_MSG("failed to allocate MessageTypeSupport_cpp"); - goto fail; + return nullptr; } _register_type(participant, info->type_support_); } @@ -128,47 +148,49 @@ create_subscription( subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); if (!get_datareader_qos(*qos_policies, subscriberParam)) { - RMW_SET_ERROR_MSG("failed to get datareader qos"); - goto fail; + return nullptr; } - info->listener_ = nullptr; + if (create_subscription_listener) { info->listener_ = new (std::nothrow) SubListener(info); if (!info->listener_) { RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); - goto fail; + return nullptr; } } + info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); if (!info->subscriber_) { RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); - goto fail; + return nullptr; } info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->subscriber_->getGuid()); - rmw_subscription = rmw_subscription_allocate(); + + rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); if (!rmw_subscription) { RMW_SET_ERROR_MSG("failed to allocate subscription"); - goto fail; + return nullptr; } + auto cleanup_subscription = rcpputils::make_scope_exit( + [rmw_subscription]() { + rmw_free(const_cast(rmw_subscription->topic_name)); + rmw_subscription_free(rmw_subscription); + }); + rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; rmw_subscription->data = info; + rmw_subscription->topic_name = rcutils_strdup(topic_name, rcutils_get_default_allocator()); if (!rmw_subscription->topic_name) { RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); - goto fail; + return nullptr; } - rmw_subscription->options = *subscription_options; - return rmw_subscription; + rmw_subscription->can_loan_messages = false; -fail: - if (info != nullptr) { - delete info->type_support_; - delete info->listener_; - delete info; - } - rmw_subscription_free(rmw_subscription); - return nullptr; + cleanup_subscription.cancel(); + cleanup_info.cancel(); + return rmw_subscription; } } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp index a27e31ce7..fad3f4ba9 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -63,15 +63,12 @@ rmw_create_subscription( const rmw_qos_profile_t * qos_policies, const rmw_subscription_options_t * subscription_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 participant_info = static_cast(node->context->impl->participant_info); @@ -102,8 +99,18 @@ rmw_create_subscription( static_cast(&msg), nullptr); if (RMW_RET_OK != rmw_ret) { - rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + rmw_error_state_t error_state = *rmw_get_error_state(); + rmw_reset_error(); + static_cast(common_context->graph_cache.dissociate_writer( + info->subscription_gid_, common_context->gid, node->name, node->namespace_)); + rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( eprosima_fastrtps_identifier, node, subscription); + 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; } } @@ -124,8 +131,15 @@ rmw_subscription_get_actual_qos( const rmw_subscription_t * subscription, rmw_qos_profile_t * qos) { - return rmw_fastrtps_shared_cpp::__rmw_subscription_get_actual_qos( - subscription, qos); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, + subscription->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); + + return rmw_fastrtps_shared_cpp::__rmw_subscription_get_actual_qos(subscription, qos); } using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; @@ -133,12 +147,21 @@ using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; rmw_ret_t rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) { - auto info = static_cast(subscription->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "subscription info pointer is null", return RMW_RET_ERROR); + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, 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( + subscription, + subscription->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + auto info = static_cast(subscription->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_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 5ea84f5a3..9f8f2ca22 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.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_subscriber_info.hpp" @@ -55,29 +58,32 @@ create_subscription( bool keyed, bool create_subscription_listener) { - (void)keyed; - (void)create_subscription_listener; - - if (!topic_name || strlen(topic_name) == 0) { - RMW_SET_ERROR_MSG("subscription 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 (!subscription_options) { - RMW_SET_ERROR_MSG("subscription_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 argument: %s", reason); + return nullptr; + } } + RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); + (void)keyed; + (void)create_subscription_listener; Participant * participant = participant_info->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } + RMW_CHECK_FOR_NULL_WITH_MSG(participant, "participant handle is null", return nullptr); const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( type_supports, rosidl_typesupport_introspection_c__identifier); @@ -94,29 +100,43 @@ create_subscription( return nullptr; } - CustomSubscriberInfo * info = nullptr; - rmw_subscription_t * rmw_subscription = nullptr; - eprosima::fastrtps::SubscriberAttributes subscriberParam; - // Load default XML profile. + eprosima::fastrtps::SubscriberAttributes subscriberParam; Domain::getDefaultSubscriberAttributes(subscriberParam); - info = new (std::nothrow) CustomSubscriberInfo(); + CustomSubscriberInfo * info = new (std::nothrow) CustomSubscriberInfo(); if (!info) { RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); return nullptr; } + auto cleanup_info = rcpputils::make_scope_exit( + [info, participant, type_support]() { + if (info->type_support_impl_) { + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + type_registry.return_message_type_support(type_support); + } + if (info->type_support_) { + _unregister_type(participant, info->type_support_); + } + if (info->subscriber_) { + if (!Domain::removeSubscriber(info->subscriber_)) { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to remove subscriber after '" + RCUTILS_STRINGIFY(__function__) "' function failed.\n"); + } + } + 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; + auto type_support_impl = type_registry.get_message_type_support(type_support); + if (!type_support_impl) { RMW_SET_ERROR_MSG("failed to allocate type support"); return nullptr; } - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_impl; + info->type_support_impl_ = type_support_impl; std::string type_name = _create_type_name( type_support->data, info->typesupport_identifier_); @@ -125,10 +145,10 @@ create_subscription( participant, type_name.c_str(), reinterpret_cast(&info->type_support_))) { - info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); + info->type_support_ = new (std::nothrow) TypeSupportProxy(type_support_impl); if (!info->type_support_) { RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); - goto fail; + return nullptr; } _register_type(participant, info->type_support_); } @@ -143,52 +163,49 @@ create_subscription( subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); if (!get_datareader_qos(*qos_policies, subscriberParam)) { - RMW_SET_ERROR_MSG("failed to get datareader qos"); - goto fail; + return nullptr; } info->listener_ = new (std::nothrow) SubListener(info); if (!info->listener_) { RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); - goto fail; + return nullptr; } info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); if (!info->subscriber_) { RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); - goto fail; + return nullptr; } info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->subscriber_->getGuid()); - rmw_subscription = rmw_subscription_allocate(); + + rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); if (!rmw_subscription) { RMW_SET_ERROR_MSG("failed to allocate subscription"); - goto fail; + return nullptr; } + auto cleanup_subscription = rcpputils::make_scope_exit( + [rmw_subscription]() { + rmw_free(const_cast(rmw_subscription->topic_name)); + rmw_subscription_free(rmw_subscription); + }); rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; rmw_subscription->data = info; + rmw_subscription->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); - if (!rmw_subscription->topic_name) { RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); - goto fail; + return nullptr; } - memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); rmw_subscription->options = *subscription_options; rmw_subscription->can_loan_messages = false; - return rmw_subscription; -fail: - if (info != nullptr) { - delete info->type_support_; - delete info->listener_; - delete info; - } - type_registry.return_message_type_support(type_support); - rmw_subscription_free(rmw_subscription); - return nullptr; + cleanup_subscription.cancel(); + cleanup_info.cancel(); + return rmw_subscription; } } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index b2416ddc3..f602dfdfa 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -38,12 +38,12 @@ struct CustomSubscriberInfo : public CustomEventInfo { virtual ~CustomSubscriberInfo() = default; - eprosima::fastrtps::Subscriber * subscriber_; - SubListener * listener_; - rmw_fastrtps_shared_cpp::TypeSupport * type_support_; - const void * type_support_impl_; - rmw_gid_t subscription_gid_; - const char * typesupport_identifier_; + eprosima::fastrtps::Subscriber * subscriber_{nullptr}; + SubListener * listener_{nullptr}; + rmw_fastrtps_shared_cpp::TypeSupport * type_support_{nullptr}; + const void * type_support_impl_{nullptr}; + rmw_gid_t subscription_gid_{}; + const char * typesupport_identifier_{nullptr}; RMW_FASTRTPS_SHARED_CPP_PUBLIC EventListenerInterface * diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index ae5932b23..95c425207 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -39,25 +39,12 @@ __rmw_destroy_subscription( const rmw_node_t * node, rmw_subscription_t * subscription) { - if (!node) { - RMW_SET_ERROR_MSG("node handle is null"); - return RMW_RET_ERROR; - } - - if (node->implementation_identifier != identifier) { - RMW_SET_ERROR_MSG("node handle not from this implementation"); - return RMW_RET_ERROR; - } - - if (!subscription) { - RMW_SET_ERROR_MSG("subscription handle is null"); - return RMW_RET_ERROR; - } - if (subscription->implementation_identifier != identifier) { - RMW_SET_ERROR_MSG("subscription handle not from this implementation"); - return RMW_RET_ERROR; - } + assert(node->implementation_identifier == identifier); + assert(subscription->implementation_identifier == identifier); + rmw_ret_t ret = RMW_RET_OK; + rmw_error_state_t error_state; + rmw_error_string_t error_string; auto common_context = static_cast(node->context->impl->common); auto info = static_cast(subscription->data); { @@ -66,22 +53,31 @@ __rmw_destroy_subscription( rmw_dds_common::msg::ParticipantEntitiesInfo msg = common_context->graph_cache.dissociate_reader( info->subscription_gid_, common_context->gid, node->name, node->namespace_); - rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, common_context->pub, static_cast(&msg), nullptr); - if (RMW_RET_OK != rmw_ret) { - return rmw_ret; + if (RMW_RET_OK != ret) { + error_state = *rmw_get_error_state(); + error_string = rmw_get_error_string(); + rmw_reset_error(); } } auto participant_info = static_cast(node->context->impl->participant_info); - return destroy_subscription( - identifier, - participant_info, - subscription); + rmw_ret_t local_ret = destroy_subscription(identifier, participant_info, subscription); + if (RMW_RET_OK != local_ret) { + if (RMW_RET_OK != ret) { + RMW_SAFE_FWRITE_TO_STDERR(error_string.str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + } + ret = local_ret; + } else if (RMW_RET_OK != ret) { + rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); + } + return ret; } rmw_ret_t @@ -104,17 +100,8 @@ __rmw_subscription_get_actual_qos( const rmw_subscription_t * subscription, rmw_qos_profile_t * qos) { - RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); - auto info = static_cast(subscription->data); - if (info == nullptr) { - return RMW_RET_ERROR; - } eprosima::fastrtps::Subscriber * fastrtps_sub = info->subscriber_; - if (fastrtps_sub == nullptr) { - return RMW_RET_ERROR; - } const eprosima::fastrtps::SubscriberAttributes & attributes = fastrtps_sub->getAttributes(); diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp index 1899bfe46..99d8bc4c8 100644 --- a/rmw_fastrtps_shared_cpp/src/subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/subscription.cpp @@ -44,35 +44,23 @@ destroy_subscription( CustomParticipantInfo * participant_info, rmw_subscription_t * subscription) { - if (!subscription) { - RMW_SET_ERROR_MSG("subscription handle is null"); - return RMW_RET_ERROR; - } - if (subscription->implementation_identifier != identifier) { - RMW_SET_ERROR_MSG("subscription 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(subscription->implementation_identifier == identifier); + rmw_ret_t ret = RMW_RET_OK; auto info = static_cast(subscription->data); - if (info != nullptr) { - if (info->subscriber_ != nullptr) { - Domain::removeSubscriber(info->subscriber_); - } - delete info->listener_; - if (info->type_support_ != nullptr) { - Participant * participant = participant_info->participant; - _unregister_type(participant, info->type_support_); - } - delete info; + if (!Domain::removeSubscriber(info->subscriber_)) { + RMW_SET_ERROR_MSG("failed to remove subscriber"); + ret = RMW_RET_ERROR; } + delete info->listener_; + + Participant * participant = participant_info->participant; + _unregister_type(participant, info->type_support_); + delete info; + rmw_free(const_cast(subscription->topic_name)); - subscription->topic_name = nullptr; rmw_subscription_free(subscription); - return RMW_RET_OK; + return ret; } } // namespace rmw_fastrtps_shared_cpp