diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index b5310cc6c..7a942a196 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -43,177 +43,167 @@ using rmw_dds_common::msg::ParticipantEntitiesInfo; static rmw_ret_t init_context_impl( - rmw_context_t* context) + rmw_context_t * context) { - rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); - rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); - - // This is currently not implemented in fastrtps - subscription_options.ignore_local_publications = true; - - std::unique_ptr common_context( - new(std::nothrow) rmw_dds_common::Context()); - if (!common_context) + rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); + rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); + + // This is currently not implemented in fastrtps + subscription_options.ignore_local_publications = true; + + std::unique_ptr common_context( + new(std::nothrow) rmw_dds_common::Context()); + if (!common_context) { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + participant_info( + rmw_fastrtps_shared_cpp::create_participant( + eprosima_fastrtps_identifier, + context->actual_domain_id, + &context->options.security_options, + (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, + context->options.enclave, + common_context.get()), + [&](CustomParticipantInfo * participant_info) { - return RMW_RET_BAD_ALLOC; - } - - std::unique_ptr> - participant_info( - rmw_fastrtps_shared_cpp::create_participant( - eprosima_fastrtps_identifier, - context->actual_domain_id, - &context->options.security_options, - (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, - context->options.enclave, - common_context.get()), - [&](CustomParticipantInfo* participant_info) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Failed to destroy participant after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!participant_info) + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy participant after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!participant_info) { + return RMW_RET_BAD_ALLOC; + } + + rmw_qos_profile_t qos = rmw_qos_profile_default; + + qos.avoid_ros_namespace_conventions = true; + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = 1; + qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + std::unique_ptr> + publisher( + rmw_fastrtps_cpp::create_publisher( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "ros_discovery_info", + &qos, + &publisher_options, + false, // our fastrtps typesupport doesn't support keyed topics + true), + [&](rmw_publisher_t * pub) { - return RMW_RET_BAD_ALLOC; - } - - rmw_qos_profile_t qos = rmw_qos_profile_default; - - qos.avoid_ros_namespace_conventions = true; - qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - qos.depth = 1; - qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - - std::unique_ptr> - publisher( - rmw_fastrtps_cpp::create_publisher( - participant_info.get(), - rosidl_typesupport_cpp::get_message_type_support_handle(), - "ros_discovery_info", - &qos, - &publisher_options, - false, // our fastrtps typesupport doesn't support keyed topics - true), - [&](rmw_publisher_t* pub) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( - eprosima_fastrtps_identifier, - participant_info.get(), - pub)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Failed to destroy publisher after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!publisher) - { - return RMW_RET_BAD_ALLOC; - } - - // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. - qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; - std::unique_ptr> - subscription( - rmw_fastrtps_cpp::create_subscription( - participant_info.get(), - rosidl_typesupport_cpp::get_message_type_support_handle(), - "ros_discovery_info", - &qos, - &subscription_options, - false), // our fastrtps typesupport doesn't support keyed topics - [&](rmw_subscription_t* sub) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( - eprosima_fastrtps_identifier, - participant_info.get(), - sub)) - { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to destroy subscription after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!subscription) + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( + eprosima_fastrtps_identifier, + participant_info.get(), + pub)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy publisher after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!publisher) { + return RMW_RET_BAD_ALLOC; + } + + // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + std::unique_ptr> + subscription( + rmw_fastrtps_cpp::create_subscription( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "ros_discovery_info", + &qos, + &subscription_options, + false), // our fastrtps typesupport doesn't support keyed topics + [&](rmw_subscription_t * sub) { - return RMW_RET_BAD_ALLOC; - } - - std::unique_ptr> - graph_guard_condition( - rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), - [&](rmw_guard_condition_t* p) - { - rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); - if (ret != RMW_RET_OK) - { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to destroy guard condition after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!graph_guard_condition) + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( + eprosima_fastrtps_identifier, + participant_info.get(), + sub)) + { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy subscription after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!subscription) { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + graph_guard_condition( + rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), + [&](rmw_guard_condition_t * p) { - return RMW_RET_BAD_ALLOC; - } - - common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, participant_info->participant_->guid()); - common_context->pub = publisher.get(); - common_context->sub = subscription.get(); - common_context->graph_guard_condition = graph_guard_condition.get(); - - context->impl->common = common_context.get(); - context->impl->participant_info = participant_info.get(); - - rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); - if (RMW_RET_OK != ret) + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); + if (ret != RMW_RET_OK) { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy guard condition after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!graph_guard_condition) { + return RMW_RET_BAD_ALLOC; + } + + common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, participant_info->participant_->guid()); + common_context->pub = publisher.get(); + common_context->sub = subscription.get(); + common_context->graph_guard_condition = graph_guard_condition.get(); + + context->impl->common = common_context.get(); + context->impl->participant_info = participant_info.get(); + + rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; + } + + common_context->graph_cache.set_on_change_callback( + [guard_condition = graph_guard_condition.get()]() { - return ret; - } - - common_context->graph_cache.set_on_change_callback( - [guard_condition = graph_guard_condition.get()]() - { - rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( - eprosima_fastrtps_identifier, - guard_condition); - }); - - common_context->graph_cache.add_participant( - common_context->gid, - context->options.enclave); - - graph_guard_condition.release(); - publisher.release(); - subscription.release(); - common_context.release(); - participant_info.release(); - return RMW_RET_OK; + rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, + guard_condition); + }); + + common_context->graph_cache.add_participant( + common_context->gid, + context->options.enclave); + + graph_guard_condition.release(); + publisher.release(); + subscription.release(); + common_context.release(); + participant_info.release(); + return RMW_RET_OK; } rmw_ret_t rmw_fastrtps_cpp::increment_context_impl_ref_count( - rmw_context_t* context) + rmw_context_t * context) { - assert(context); - assert(context->impl); + assert(context); + assert(context->impl); - std::lock_guard guard(context->impl->mutex); + std::lock_guard guard(context->impl->mutex); - if (!context->impl->count) - { - rmw_ret_t ret = init_context_impl(context); - if (RMW_RET_OK != ret) - { - return ret; - } + if (!context->impl->count) { + rmw_ret_t ret = init_context_impl(context); + if (RMW_RET_OK != ret) { + return ret; } - context->impl->count++; - return RMW_RET_OK; + } + context->impl->count++; + return RMW_RET_OK; } diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 1880117c7..57ab05787 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -281,7 +281,8 @@ rmw_fastrtps_cpp::create_publisher( return nullptr; } - info->data_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + info->data_writer_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::none()); // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 1973ff126..e770c9f05 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -337,7 +337,8 @@ rmw_create_client( return nullptr; } - info->response_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + info->response_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( @@ -390,7 +391,8 @@ rmw_create_client( return nullptr; } - info->request_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + info->request_writer_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::none()); // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 263d68bff..4fe87afa8 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -336,7 +336,8 @@ rmw_create_service( return nullptr; } - info->request_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + info->request_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( @@ -393,7 +394,8 @@ rmw_create_service( return nullptr; } - info->response_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + info->response_writer_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::none()); // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index ff3d7814b..9ac46c332 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -55,319 +55,299 @@ using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; -namespace rmw_fastrtps_cpp { +namespace rmw_fastrtps_cpp +{ -rmw_subscription_t* +rmw_subscription_t * create_subscription( - const CustomParticipantInfo* participant_info, - const rosidl_message_type_support_t* type_supports, - const char* topic_name, - const rmw_qos_profile_t* qos_policies, - const rmw_subscription_options_t* subscription_options, - bool keyed) + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_subscription_options_t * subscription_options, + bool keyed) { - ///// - // Check input parameters - RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); - - 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("create_subscription() called with an empty topic_name argument"); - 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( - "create_subscription() called with invalid topic name: %s", reason); - return nullptr; - } - } - RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); - - ///// - // Check RMW QoS - if (!is_valid_qos(*qos_policies)) - { - RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); - return nullptr; - } - - ///// - // Get RMW Type Support - const rosidl_message_type_support_t* type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); - if (!type_support) - { - rcutils_error_string_t prev_error_string = rcutils_get_error_string(); - rcutils_reset_error(); - type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); - if (!type_support) - { - rcutils_error_string_t error_string = rcutils_get_error_string(); - rcutils_reset_error(); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Type support not from this implementation. Got:\n" - " %s\n" - " %s\n" - "while fetching it", - prev_error_string.str, error_string.str); - return nullptr; - } - } - - std::lock_guard lck(participant_info->entity_creation_mutex_); - - ///// - // Find and check existing topic and type - - // Create Topic and Type names - auto callbacks = static_cast(type_support->data); - std::string type_name = _create_type_name(callbacks); - auto topic_name_mangled = - _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); - - eprosima::fastdds::dds::TypeSupport fastdds_type; - eprosima::fastdds::dds::TopicDescription* des_topic = nullptr; - if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( - participant_info, - topic_name_mangled, - type_name, - &des_topic, - &fastdds_type)) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "create_subscription() called for existing topic name %s with incompatible type %s", - topic_name_mangled.c_str(), type_name.c_str()); - return nullptr; - } - - ///// - // Get Participant and Subscriber - eprosima::fastdds::dds::DomainParticipant* dds_participant = participant_info->participant_; - eprosima::fastdds::dds::Subscriber* subscriber = participant_info->subscriber_; - - ///// - // Create the custom Subscriber struct (info) - auto info = new (std::nothrow) CustomSubscriberInfo(); - if (!info) - { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); - return nullptr; - } - - auto cleanup_info = rcpputils::make_scope_exit( - [info, dds_participant]() - { - delete info->listener_; - if (info->type_support_) - { - dds_participant->unregister_type(info->type_support_.get_type_name()); - } - delete info; - }); - - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = callbacks; - - ///// - // Create the Type Support struct - if (!fastdds_type) - { - auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks); - if (!tsupport) - { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); - return nullptr; - } - - // Transfer ownership to fastdds_type - fastdds_type.reset(tsupport); - } - - if (keyed && !fastdds_type->m_isGetKeyDefined) - { - RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); - return nullptr; - } - - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) - { - RMW_SET_ERROR_MSG("create_subscription() failed to register type"); - return nullptr; - } - info->type_support_ = fastdds_type; - - if (!rmw_fastrtps_shared_cpp::register_type_object(type_supports, type_name)) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "failed to register type object with incompatible type %s", - type_name.c_str()); - return nullptr; + ///// + // Check input parameters + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); + + 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("create_subscription() called with an empty topic_name argument"); + 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; } - - ///// - // Create Listener - info->listener_ = new (std::nothrow) SubListener(info); - if (!info->listener_) - { - RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); - 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( + "create_subscription() called with invalid topic name: %s", reason); + return nullptr; } - - ///// - // Create and register Topic - eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topic_qos)) - { - RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); - return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); + + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); + return nullptr; + } + + ///// + // Get RMW Type Support + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); + if (!type_support) { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); + if (!type_support) { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; } - - rmw_fastrtps_shared_cpp::TopicHolder topic; - if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - dds_participant, des_topic, - topic_name_mangled, type_name, topic_qos, false, &topic)) - { - RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); - return nullptr; - } - - info->dds_participant_ = dds_participant; - info->subscriber_ = subscriber; - info->topic_name_mangled_ = topic_name_mangled; - info->topic_ = topic.desc; - des_topic = topic.desc; - - // Create ContentFilteredTopic - if (subscription_options->content_filter_options) + } + + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topic and type + + // Create Topic and Type names + auto callbacks = static_cast(type_support->data); + std::string type_name = _create_type_name(callbacks); + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + eprosima::fastdds::dds::TypeSupport fastdds_type; + eprosima::fastdds::dds::TopicDescription * des_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + topic_name_mangled, + type_name, + &des_topic, + &fastdds_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_subscription() called for existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); + return nullptr; + } + + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + + ///// + // Create the custom Subscriber struct (info) + auto info = new (std::nothrow) CustomSubscriberInfo(); + if (!info) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); + return nullptr; + } + + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() { - rmw_subscription_content_filter_options_t* options = - subscription_options->content_filter_options; - if (nullptr != options->filter_expression) - { - eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic = nullptr; - if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( - dds_participant, des_topic, - topic_name_mangled, options, &filtered_topic)) - { - RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); - return nullptr; - } - info->filtered_topic_ = filtered_topic; - des_topic = filtered_topic; - } + delete info->listener_; + if (info->type_support_) { + dds_participant->unregister_type(info->type_support_.get_type_name()); + } + delete info; + }); + + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = callbacks; + + ///// + // Create the Type Support struct + if (!fastdds_type) { + auto tsupport = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate MessageTypeSupport"); + return nullptr; } - ///// - // Create DataReader - - // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load - // datareader which profile name matches with topic_name. If such profile does not exist, - // then use the default Fast DDS QoS. - eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); - - // Try to load the profile with the topic name - // It does not need to check the return code, as if the profile does not exist, - // the QoS is already the default - subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); - - if (!participant_info->leave_middleware_default_qos) - { - reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - - reader_qos.data_sharing().off(); - } - - if (!get_datareader_qos(*qos_policies, reader_qos)) - { - RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); - return nullptr; - } - - info->datareader_qos_ = reader_qos; - - // create_datareader - if (!rmw_fastrtps_shared_cpp::create_datareader( - info->datareader_qos_, - subscription_options, - subscriber, - des_topic, - info->listener_, - &info->data_reader_)) - { - RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); + } + + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); + return nullptr; + } + + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + RMW_SET_ERROR_MSG("create_subscription() failed to register type"); + return nullptr; + } + info->type_support_ = fastdds_type; + + if (!rmw_fastrtps_shared_cpp::register_type_object(type_supports, type_name)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "failed to register type object with incompatible type %s", + type_name.c_str()); + return nullptr; + } + + ///// + // Create Listener + info->listener_ = new (std::nothrow) SubListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } + + ///// + // Create and register Topic + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); + return nullptr; + } + + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, des_topic, + topic_name_mangled, type_name, topic_qos, false, &topic)) + { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); + return nullptr; + } + + info->dds_participant_ = dds_participant; + info->subscriber_ = subscriber; + info->topic_name_mangled_ = topic_name_mangled; + info->topic_ = topic.desc; + des_topic = topic.desc; + + // Create ContentFilteredTopic + if (subscription_options->content_filter_options) { + rmw_subscription_content_filter_options_t * options = + subscription_options->content_filter_options; + if (nullptr != options->filter_expression) { + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( + dds_participant, des_topic, + topic_name_mangled, options, &filtered_topic)) + { + RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); return nullptr; + } + info->filtered_topic_ = filtered_topic; + des_topic = filtered_topic; } - - // Initialize DataReader's StatusCondition to - info->data_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); - - // lambda to delete datareader - auto cleanup_datareader = rcpputils::make_scope_exit( - [subscriber, info]() - { - subscriber->delete_datareader(info->data_reader_); - }); - - ///// - // Create RMW GID - info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->data_reader_->guid()); - - ///// - // Allocate subscription - rmw_subscription_t* rmw_subscription = rmw_subscription_allocate(); - if (!rmw_subscription) + } + + ///// + // Create DataReader + + // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load + // datareader which profile name matches with topic_name. If such profile does not exist, + // then use the default Fast DDS QoS. + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); + + // Try to load the profile with the topic name + // It does not need to check the return code, as if the profile does not exist, + // the QoS is already the default + subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); + + if (!participant_info->leave_middleware_default_qos) { + reader_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + reader_qos.data_sharing().off(); + } + + if (!get_datareader_qos(*qos_policies, reader_qos)) { + RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); + return nullptr; + } + + info->datareader_qos_ = reader_qos; + + // create_datareader + if (!rmw_fastrtps_shared_cpp::create_datareader( + info->datareader_qos_, + subscription_options, + subscriber, + des_topic, + info->listener_, + &info->data_reader_)) + { + RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); + return nullptr; + } + + // Initialize DataReader's StatusCondition to + info->data_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); - return nullptr; - } - auto cleanup_rmw_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) + subscriber->delete_datareader(info->data_reader_); + }); + + ///// + // Create RMW GID + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->data_reader_->guid()); + + ///// + // Allocate subscription + rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); + if (!rmw_subscription) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); + return nullptr; + } + auto cleanup_rmw_subscription = rcpputils::make_scope_exit( + [rmw_subscription]() { - RMW_SET_ERROR_MSG( - "create_subscription() failed to allocate memory for subscription topic name"); - return nullptr; - } - rmw_subscription->options = *subscription_options; - rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); - rmw_subscription->is_cft_enabled = info->filtered_topic_ != nullptr; - - topic.should_be_deleted = false; - cleanup_rmw_subscription.cancel(); - cleanup_datareader.cancel(); - cleanup_info.cancel(); - - TRACEPOINT( - rmw_subscription_init, - static_cast(rmw_subscription), - info->subscription_gid_.data); - return 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( + "create_subscription() failed to allocate memory for subscription topic name"); + return nullptr; + } + rmw_subscription->options = *subscription_options; + rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); + rmw_subscription->is_cft_enabled = info->filtered_topic_ != nullptr; + + topic.should_be_deleted = false; + cleanup_rmw_subscription.cancel(); + cleanup_datareader.cancel(); + cleanup_info.cancel(); + + TRACEPOINT( + rmw_subscription_init, + static_cast(rmw_subscription), + info->subscription_gid_.data); + return rmw_subscription; } } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index 1e37ff6b8..5866e05de 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -43,177 +43,167 @@ using rmw_dds_common::msg::ParticipantEntitiesInfo; static rmw_ret_t init_context_impl( - rmw_context_t* context) + rmw_context_t * context) { - rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); - rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); - - // This is currently not implemented in fastrtps - subscription_options.ignore_local_publications = true; - - std::unique_ptr common_context( - new(std::nothrow) rmw_dds_common::Context()); - if (!common_context) + rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); + rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); + + // This is currently not implemented in fastrtps + subscription_options.ignore_local_publications = true; + + std::unique_ptr common_context( + new(std::nothrow) rmw_dds_common::Context()); + if (!common_context) { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + participant_info( + rmw_fastrtps_shared_cpp::create_participant( + eprosima_fastrtps_identifier, + context->actual_domain_id, + &context->options.security_options, + (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, + context->options.enclave, + common_context.get()), + [&](CustomParticipantInfo * participant_info) { - return RMW_RET_BAD_ALLOC; - } - - std::unique_ptr> - participant_info( - rmw_fastrtps_shared_cpp::create_participant( - eprosima_fastrtps_identifier, - context->actual_domain_id, - &context->options.security_options, - (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, - context->options.enclave, - common_context.get()), - [&](CustomParticipantInfo* participant_info) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Failed to destroy participant after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!participant_info) + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy participant after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!participant_info) { + return RMW_RET_BAD_ALLOC; + } + + rmw_qos_profile_t qos = rmw_qos_profile_default; + + qos.avoid_ros_namespace_conventions = true; + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = 1; + qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + std::unique_ptr> + publisher( + rmw_fastrtps_dynamic_cpp::create_publisher( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "ros_discovery_info", + &qos, + &publisher_options, + false, // our fastrtps typesupport doesn't support keyed topics + true), + [&](rmw_publisher_t * pub) { - return RMW_RET_BAD_ALLOC; - } - - rmw_qos_profile_t qos = rmw_qos_profile_default; - - qos.avoid_ros_namespace_conventions = true; - qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - qos.depth = 1; - qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - - std::unique_ptr> - publisher( - rmw_fastrtps_dynamic_cpp::create_publisher( - participant_info.get(), - rosidl_typesupport_cpp::get_message_type_support_handle(), - "ros_discovery_info", - &qos, - &publisher_options, - false, // our fastrtps typesupport doesn't support keyed topics - true), - [&](rmw_publisher_t* pub) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( - eprosima_fastrtps_identifier, - participant_info.get(), - pub)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR( - "Failed to destroy publisher after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!publisher) - { - return RMW_RET_BAD_ALLOC; - } - - // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. - qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; - std::unique_ptr> - subscription( - rmw_fastrtps_dynamic_cpp::create_subscription( - participant_info.get(), - rosidl_typesupport_cpp::get_message_type_support_handle(), - "ros_discovery_info", - &qos, - &subscription_options, - false), // our fastrtps typesupport doesn't support keyed topics - [&](rmw_subscription_t* sub) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( - eprosima_fastrtps_identifier, - participant_info.get(), - sub)) - { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to destroy subscription after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!subscription) + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( + eprosima_fastrtps_identifier, + participant_info.get(), + pub)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy publisher after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!publisher) { + return RMW_RET_BAD_ALLOC; + } + + // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + std::unique_ptr> + subscription( + rmw_fastrtps_dynamic_cpp::create_subscription( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "ros_discovery_info", + &qos, + &subscription_options, + false), // our fastrtps typesupport doesn't support keyed topics + [&](rmw_subscription_t * sub) { - return RMW_RET_BAD_ALLOC; - } - - std::unique_ptr> - graph_guard_condition( - rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), - [&](rmw_guard_condition_t* p) - { - rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); - if (ret != RMW_RET_OK) - { - RMW_SAFE_FWRITE_TO_STDERR( - "Failed to destroy guard condition after function: '" - RCUTILS_STRINGIFY(__function__) "' failed.\n"); - } - }); - if (!graph_guard_condition) + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( + eprosima_fastrtps_identifier, + participant_info.get(), + sub)) + { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy subscription after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!subscription) { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + graph_guard_condition( + rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), + [&](rmw_guard_condition_t * p) { - return RMW_RET_BAD_ALLOC; - } - - common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, participant_info->participant_->guid()); - common_context->pub = publisher.get(); - common_context->sub = subscription.get(); - common_context->graph_guard_condition = graph_guard_condition.get(); - - context->impl->common = common_context.get(); - context->impl->participant_info = participant_info.get(); - - rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); - if (RMW_RET_OK != ret) + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); + if (ret != RMW_RET_OK) { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy guard condition after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!graph_guard_condition) { + return RMW_RET_BAD_ALLOC; + } + + common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, participant_info->participant_->guid()); + common_context->pub = publisher.get(); + common_context->sub = subscription.get(); + common_context->graph_guard_condition = graph_guard_condition.get(); + + context->impl->common = common_context.get(); + context->impl->participant_info = participant_info.get(); + + rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; + } + + common_context->graph_cache.set_on_change_callback( + [guard_condition = graph_guard_condition.get()]() { - return ret; - } - - common_context->graph_cache.set_on_change_callback( - [guard_condition = graph_guard_condition.get()]() - { - rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( - eprosima_fastrtps_identifier, - guard_condition); - }); - - common_context->graph_cache.add_participant( - common_context->gid, - context->options.enclave); - - graph_guard_condition.release(); - publisher.release(); - subscription.release(); - common_context.release(); - participant_info.release(); - return RMW_RET_OK; + rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, + guard_condition); + }); + + common_context->graph_cache.add_participant( + common_context->gid, + context->options.enclave); + + graph_guard_condition.release(); + publisher.release(); + subscription.release(); + common_context.release(); + participant_info.release(); + return RMW_RET_OK; } rmw_ret_t rmw_fastrtps_dynamic_cpp::increment_context_impl_ref_count( - rmw_context_t* context) + rmw_context_t * context) { - assert(context); - assert(context->impl); + assert(context); + assert(context->impl); - std::lock_guard guard(context->impl->mutex); + std::lock_guard guard(context->impl->mutex); - if (!context->impl->count) - { - rmw_ret_t ret = init_context_impl(context); - if (RMW_RET_OK != ret) - { - return ret; - } + if (!context->impl->count) { + rmw_ret_t ret = init_context_impl(context); + if (RMW_RET_OK != ret) { + return ret; } - context->impl->count++; - return RMW_RET_OK; + } + context->impl->count++; + return RMW_RET_OK; } diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 9fa07547f..83c656404 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -286,7 +286,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( return nullptr; } - info->data_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + info->data_writer_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::none()); // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 956450843..2ad73fe71 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -368,7 +368,8 @@ rmw_create_client( return nullptr; } - info->response_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + info->response_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( @@ -421,7 +422,8 @@ rmw_create_client( return nullptr; } - info->request_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + info->request_writer_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::none()); // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 49985c55b..91fdeab6e 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -367,7 +367,8 @@ rmw_create_service( return nullptr; } - info->request_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); + info->request_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); // lambda to delete datareader auto cleanup_datareader = rcpputils::make_scope_exit( @@ -424,7 +425,8 @@ rmw_create_service( return nullptr; } - info->response_writer_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::none()); + info->response_writer_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::none()); // lambda to delete datawriter auto cleanup_datawriter = rcpputils::make_scope_exit( diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp index 8d54cdf61..447d3f51c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -55,335 +55,315 @@ using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; -namespace rmw_fastrtps_dynamic_cpp { +namespace rmw_fastrtps_dynamic_cpp +{ -rmw_subscription_t* +rmw_subscription_t * create_subscription( - const CustomParticipantInfo* participant_info, - const rosidl_message_type_support_t* type_supports, - const char* topic_name, - const rmw_qos_profile_t* qos_policies, - const rmw_subscription_options_t* subscription_options, - bool keyed) + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_subscription_options_t * subscription_options, + bool keyed) { - RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); - - ///// - // Check input parameters - 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("create_subscription() called with an empty topic_name argument"); - 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( - "create_subscription() called with invalid topic name: %s", reason); - return nullptr; - } + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); + + ///// + // Check input parameters + 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("create_subscription() called with an empty topic_name argument"); + 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; } - RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); - - ///// - // Check RMW QoS - if (!is_valid_qos(*qos_policies)) - { - RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); - return nullptr; - } - - ///// - // Get RMW Type Support - const rosidl_message_type_support_t* type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); - if (!type_support) - { - rcutils_error_string_t prev_error_string = rcutils_get_error_string(); - rcutils_reset_error(); - type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (!type_support) - { - rcutils_error_string_t error_string = rcutils_get_error_string(); - rcutils_reset_error(); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Type support not from this implementation. Got:\n" - " %s\n" - " %s\n" - "while fetching it", - prev_error_string.str, error_string.str); - 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( + "create_subscription() called with invalid topic name: %s", reason); + return nullptr; } - - std::lock_guard lck(participant_info->entity_creation_mutex_); - - ///// - // Find and check existing topic and type - - // Create Topic and Type names - std::string type_name = _create_type_name( - type_support->data, type_support->typesupport_identifier); - auto topic_name_mangled = - _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); - - eprosima::fastdds::dds::TypeSupport fastdds_type; - eprosima::fastdds::dds::TopicDescription* des_topic; - if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( - participant_info, - topic_name_mangled, - type_name, - &des_topic, - &fastdds_type)) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "create_subscription() called with existing topic name %s with incompatible type %s", - topic_name_mangled.c_str(), type_name.c_str()); - return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); + + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); + return nullptr; + } + + ///// + // Get RMW Type Support + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; } - - ///// - // Get Participant and Subscriber - eprosima::fastdds::dds::DomainParticipant* dds_participant = participant_info->participant_; - eprosima::fastdds::dds::Subscriber* subscriber = participant_info->subscriber_; - - ///// - // Create the custom Subscriber struct (info) - auto info = new (std::nothrow) CustomSubscriberInfo(); - if (!info) + } + + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topic and type + + // Create Topic and Type names + std::string type_name = _create_type_name( + type_support->data, type_support->typesupport_identifier); + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + eprosima::fastdds::dds::TypeSupport fastdds_type; + eprosima::fastdds::dds::TopicDescription * des_topic; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + topic_name_mangled, + type_name, + &des_topic, + &fastdds_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_subscription() called with existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); + return nullptr; + } + + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + + ///// + // Create the custom Subscriber struct (info) + auto info = new (std::nothrow) CustomSubscriberInfo(); + if (!info) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); + return nullptr; + } + + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); - return nullptr; - } - - auto cleanup_info = rcpputils::make_scope_exit( - [info, dds_participant]() - { - delete info->listener_; - if (info->type_support_) - { - dds_participant->unregister_type(info->type_support_.get_type_name()); - } - delete info; - }); - - ///// - // Create the Type Support struct - TypeSupportRegistry& type_registry = TypeSupportRegistry::get_instance(); - auto type_support_impl = type_registry.get_message_type_support(type_support); - if (!type_support_impl) + delete info->listener_; + if (info->type_support_) { + dds_participant->unregister_type(info->type_support_.get_type_name()); + } + delete info; + }); + + ///// + // Create the Type Support struct + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto type_support_impl = type_registry.get_message_type_support(type_support); + if (!type_support_impl) { + RMW_SET_ERROR_MSG("create_subscription() failed to get message_type_support"); + return nullptr; + } + auto return_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { - RMW_SET_ERROR_MSG("create_subscription() failed to get message_type_support"); - return nullptr; - } - auto return_type_support = rcpputils::make_scope_exit( - [&type_registry, type_support]() - { - type_registry.return_message_type_support(type_support); - }); + type_registry.return_message_type_support(type_support); + }); - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support_impl; + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_support_impl; - if (!fastdds_type) - { - auto tsupport = new (std::nothrow) TypeSupportProxy(type_support_impl); - if (!tsupport) - { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate TypeSupportProxy"); - return nullptr; - } - - // Transfer ownership to fastdds_type - fastdds_type.reset(tsupport); - } - - if (keyed && !fastdds_type->m_isGetKeyDefined) - { - RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); - return nullptr; - } - - if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) - { - RMW_SET_ERROR_MSG("create_subscription() failed to register type"); - return nullptr; - } - - info->type_support_ = fastdds_type; - - ///// - // Create Listener - info->listener_ = new (std::nothrow) SubListener(info); - - if (!info->listener_) - { - RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); - return nullptr; - } - - ///// - // Create and register Topic - eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); - if (!get_topic_qos(*qos_policies, topic_qos)) - { - RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); - return nullptr; + if (!fastdds_type) { + auto tsupport = new (std::nothrow) TypeSupportProxy(type_support_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate TypeSupportProxy"); + return nullptr; } - rmw_fastrtps_shared_cpp::TopicHolder topic; - if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( - dds_participant, des_topic, - topic_name_mangled, type_name, topic_qos, false, &topic)) - { - RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); - return nullptr; - } - - des_topic = topic.desc; - - ///// - // Create DataReader - - // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load - // datareader which profile name matches with topic_name. If such profile does not exist, - // then use the default Fast DDS QoS. - eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); - - // Try to load the profile with the topic name - // It does not need to check the return code, as if the profile does not exist, - // the QoS is already the default - subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); - - if (!participant_info->leave_middleware_default_qos) - { - reader_qos.endpoint().history_memory_policy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - - reader_qos.data_sharing().off(); - } - - if (!get_datareader_qos(*qos_policies, reader_qos)) - { - RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); - return nullptr; - } - - info->listener_ = new (std::nothrow) SubListener(info); - if (!info->listener_) - { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); - return nullptr; - } - - eprosima::fastdds::dds::DataReaderQos original_qos = reader_qos; - switch (subscription_options->require_unique_network_flow_endpoints){ - default: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_SYSTEM_DEFAULT: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED: - // Unique network flow endpoints not required. We leave the decission to the XML profile. - break; - - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED: - case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED: - // Ensure we request unique network flow endpoints - if (nullptr == - PropertyPolicyHelper::find_property( - reader_qos.properties(), - "fastdds.unique_network_flows")) - { - reader_qos.properties().properties().emplace_back("fastdds.unique_network_flows", ""); - } - break; - } - - // Creates DataReader (with subscriber name to not change name policy) + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); + } + + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); + return nullptr; + } + + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + RMW_SET_ERROR_MSG("create_subscription() failed to register type"); + return nullptr; + } + + info->type_support_ = fastdds_type; + + ///// + // Create Listener + info->listener_ = new (std::nothrow) SubListener(info); + + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } + + ///// + // Create and register Topic + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); + return nullptr; + } + + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, des_topic, + topic_name_mangled, type_name, topic_qos, false, &topic)) + { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); + return nullptr; + } + + des_topic = topic.desc; + + ///// + // Create DataReader + + // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load + // datareader which profile name matches with topic_name. If such profile does not exist, + // then use the default Fast DDS QoS. + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); + + // Try to load the profile with the topic name + // It does not need to check the return code, as if the profile does not exist, + // the QoS is already the default + subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); + + if (!participant_info->leave_middleware_default_qos) { + reader_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + reader_qos.data_sharing().off(); + } + + if (!get_datareader_qos(*qos_policies, reader_qos)) { + RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); + return nullptr; + } + + info->listener_ = new (std::nothrow) SubListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); + return nullptr; + } + + eprosima::fastdds::dds::DataReaderQos original_qos = reader_qos; + switch (subscription_options->require_unique_network_flow_endpoints) { + default: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_SYSTEM_DEFAULT: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED: + // Unique network flow endpoints not required. We leave the decission to the XML profile. + break; + + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED: + // Ensure we request unique network flow endpoints + if (nullptr == + PropertyPolicyHelper::find_property( + reader_qos.properties(), + "fastdds.unique_network_flows")) + { + reader_qos.properties().properties().emplace_back("fastdds.unique_network_flows", ""); + } + break; + } + + // Creates DataReader (with subscriber name to not change name policy) + info->data_reader_ = subscriber->create_datareader( + des_topic, + reader_qos, + info->listener_, + eprosima::fastdds::dds::StatusMask::subscription_matched()); + if (!info->data_reader_ && + (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == + subscription_options->require_unique_network_flow_endpoints)) + { info->data_reader_ = subscriber->create_datareader( - des_topic, - reader_qos, - info->listener_, - eprosima::fastdds::dds::StatusMask::subscription_matched()); - if (!info->data_reader_ && - (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == - subscription_options->require_unique_network_flow_endpoints)) + des_topic, + original_qos, + info->listener_, + eprosima::fastdds::dds::StatusMask::subscription_matched()); + } + + if (!info->data_reader_) { + RMW_SET_ERROR_MSG("create_subscription() could not create data reader"); + return nullptr; + } + + // Initialize DataReader's StatusCondition to + info->data_reader_->get_statuscondition().set_enabled_statuses( + eprosima::fastdds::dds::StatusMask::data_available()); + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { - info->data_reader_ = subscriber->create_datareader( - des_topic, - original_qos, - info->listener_, - eprosima::fastdds::dds::StatusMask::subscription_matched()); - } - - if (!info->data_reader_) + subscriber->delete_datareader(info->data_reader_); + }); + + ///// + // Create RMW GID + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->data_reader_->guid()); + + rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); + if (!rmw_subscription) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); + return nullptr; + } + auto cleanup_rmw_subscription = rcpputils::make_scope_exit( + [rmw_subscription]() { - RMW_SET_ERROR_MSG("create_subscription() could not create data reader"); - return nullptr; - } - - // Initialize DataReader's StatusCondition to - info->data_reader_->get_statuscondition().set_enabled_statuses(eprosima::fastdds::dds::StatusMask::data_available()); - - // lambda to delete datareader - auto cleanup_datareader = rcpputils::make_scope_exit( - [subscriber, info]() - { - subscriber->delete_datareader(info->data_reader_); - }); - - ///// - // Create RMW GID - info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->data_reader_->guid()); - - rmw_subscription_t* rmw_subscription = rmw_subscription_allocate(); - if (!rmw_subscription) - { - RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); - return nullptr; - } - auto cleanup_rmw_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( - "create_subscription() failed to allocate memory for subscription topic name"); - return nullptr; - } - memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); - - rmw_subscription->options = *subscription_options; - rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); - // TODO(iuhilnehc-ynos): update after rmw_fastrtps_cpp is confirmed - rmw_subscription->is_cft_enabled = false; - - topic.should_be_deleted = false; - cleanup_rmw_subscription.cancel(); - cleanup_datareader.cancel(); - return_type_support.cancel(); - cleanup_info.cancel(); - return 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( + "create_subscription() failed to allocate memory for subscription topic name"); + return nullptr; + } + memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); + + rmw_subscription->options = *subscription_options; + rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); + // TODO(iuhilnehc-ynos): update after rmw_fastrtps_cpp is confirmed + rmw_subscription->is_cft_enabled = false; + + topic.should_be_deleted = false; + cleanup_rmw_subscription.cancel(); + cleanup_datareader.cancel(); + return_type_support.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_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index 9a75b9149..d5a38b470 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -48,192 +48,173 @@ class ClientPubListener; typedef struct CustomClientInfo { - eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; - const void* request_type_support_impl_{nullptr}; - eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; - const void* response_type_support_impl_{nullptr}; - eprosima::fastdds::dds::DataReader* response_reader_{nullptr}; - eprosima::fastdds::dds::DataWriter* request_writer_{nullptr}; - - std::string request_topic_; - std::string response_topic_; - - ClientListener* listener_{nullptr}; - eprosima::fastrtps::rtps::GUID_t writer_guid_; - eprosima::fastrtps::rtps::GUID_t reader_guid_; - - const char* typesupport_identifier_{nullptr}; - ClientPubListener* pub_listener_{nullptr}; - std::atomic_size_t response_subscriber_matched_count_; - std::atomic_size_t request_publisher_matched_count_; + eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; + const void * request_type_support_impl_{nullptr}; + eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; + const void * response_type_support_impl_{nullptr}; + eprosima::fastdds::dds::DataReader * response_reader_{nullptr}; + eprosima::fastdds::dds::DataWriter * request_writer_{nullptr}; + + std::string request_topic_; + std::string response_topic_; + + ClientListener * listener_{nullptr}; + eprosima::fastrtps::rtps::GUID_t writer_guid_; + eprosima::fastrtps::rtps::GUID_t reader_guid_; + + const char * typesupport_identifier_{nullptr}; + ClientPubListener * pub_listener_{nullptr}; + std::atomic_size_t response_subscriber_matched_count_; + std::atomic_size_t request_publisher_matched_count_; } CustomClientInfo; typedef struct CustomClientResponse { - eprosima::fastrtps::rtps::SampleIdentity sample_identity_; - std::unique_ptr buffer_; - eprosima::fastdds::dds::SampleInfo sample_info_ {}; + eprosima::fastrtps::rtps::SampleIdentity sample_identity_; + std::unique_ptr buffer_; + eprosima::fastdds::dds::SampleInfo sample_info_ {}; } CustomClientResponse; class ClientListener : public eprosima::fastdds::dds::DataReaderListener { public: - - explicit ClientListener( - CustomClientInfo* info) - : info_(info) - { + explicit ClientListener( + CustomClientInfo * info) + : info_(info) + { + } + + void + on_data_available( + eprosima::fastdds::dds::DataReader *) + { + std::unique_lock lock_mutex(on_new_response_m_); + + if (on_new_response_cb_) { + auto unread_responses = get_unread_responses(); + + if (0 < unread_responses) { + on_new_response_cb_(user_data_, unread_responses); + } } - - void - on_data_available( - eprosima::fastdds::dds::DataReader*) - { - std::unique_lock lock_mutex(on_new_response_m_); - - if (on_new_response_cb_) - { - auto unread_responses = get_unread_responses(); - - if (0 < unread_responses) - { - on_new_response_cb_(user_data_, unread_responses); - } - } + } + + void on_subscription_matched( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final + { + if (info_ == nullptr) { + return; } - - void on_subscription_matched( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) final - { - if (info_ == nullptr) - { - return; - } - if (info.current_count_change == 1) - { - publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } - else if (info.current_count_change == -1) - { - publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } - else - { - return; - } - info_->response_subscriber_matched_count_.store(publishers_.size()); + if (info.current_count_change == 1) { + publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } else if (info.current_count_change == -1) { + publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } else { + return; } - - size_t get_unread_responses() + info_->response_subscriber_matched_count_.store(publishers_.size()); + } + + size_t get_unread_responses() + { + size_t ret_value = 0; + eprosima::fastdds::dds::LoanableSequence data_seq; + eprosima::fastdds::dds::SampleInfoSeq info_seq; + + if (ReturnCode_t::RETCODE_OK == info_->response_reader_->read( + data_seq, info_seq, + eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) { - size_t ret_value = 0; - eprosima::fastdds::dds::LoanableSequence data_seq; - eprosima::fastdds::dds::SampleInfoSeq info_seq; - - if (ReturnCode_t::RETCODE_OK == info_->response_reader_->read(data_seq, info_seq, - eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) - { - for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; count < info_seq.length(); ++count) - { - if (info_seq[count].valid_data) - { - ++ret_value; - } - } - - info_->response_reader_->return_loan(data_seq, info_seq); + for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; + count < info_seq.length(); ++count) + { + if (info_seq[count].valid_data) { + ++ret_value; } + } - return ret_value; + info_->response_reader_->return_loan(data_seq, info_seq); } - // Provide handlers to perform an action when a - // new event from this listener has ocurred - void - set_on_new_response_callback( - const void* user_data, - rmw_event_callback_t callback) - { - std::unique_lock lock_mutex(on_new_response_m_); - - if (callback) - { - auto unread_responses = get_unread_responses(); - - if (0 < unread_responses) - { - callback(user_data_, unread_responses); - } - - user_data_ = user_data; - on_new_response_cb_ = callback; - - eprosima::fastdds::dds::StatusMask status_mask = info_->response_reader_->get_status_mask(); - info_->response_reader_->set_listener(this, - status_mask << eprosima::fastdds::dds::StatusMask::data_available()); - } - else - { - eprosima::fastdds::dds::StatusMask status_mask = info_->response_reader_->get_status_mask(); - info_->response_reader_->set_listener(this, - status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); - - user_data_ = nullptr; - on_new_response_cb_ = nullptr; - } + return ret_value; + } + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + void + set_on_new_response_callback( + const void * user_data, + rmw_event_callback_t callback) + { + std::unique_lock lock_mutex(on_new_response_m_); + + if (callback) { + auto unread_responses = get_unread_responses(); + + if (0 < unread_responses) { + callback(user_data_, unread_responses); + } + + user_data_ = user_data; + on_new_response_cb_ = callback; + + eprosima::fastdds::dds::StatusMask status_mask = info_->response_reader_->get_status_mask(); + info_->response_reader_->set_listener( + this, + status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + } else { + eprosima::fastdds::dds::StatusMask status_mask = info_->response_reader_->get_status_mask(); + info_->response_reader_->set_listener( + this, + status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); + + user_data_ = nullptr; + on_new_response_cb_ = nullptr; } + } private: + CustomClientInfo * info_; - CustomClientInfo* info_; - - std::set publishers_; + std::set publishers_; - rmw_event_callback_t on_new_response_cb_{nullptr}; + rmw_event_callback_t on_new_response_cb_{nullptr}; - const void* user_data_{nullptr}; + const void * user_data_{nullptr}; - std::mutex on_new_response_m_; + std::mutex on_new_response_m_; }; class ClientPubListener : public eprosima::fastdds::dds::DataWriterListener { public: - - explicit ClientPubListener( - CustomClientInfo* info) - : info_(info) - { + explicit ClientPubListener( + CustomClientInfo * info) + : info_(info) + { + } + + void on_publication_matched( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus & info) final + { + if (info_ == nullptr) { + return; } - - void on_publication_matched( - eprosima::fastdds::dds::DataWriter* /* writer */, - const eprosima::fastdds::dds::PublicationMatchedStatus& info) final - { - if (info_ == nullptr) - { - return; - } - if (info.current_count_change == 1) - { - subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } - else if (info.current_count_change == -1) - { - subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } - else - { - return; - } - info_->request_publisher_matched_count_.store(subscriptions_.size()); + if (info.current_count_change == 1) { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else if (info.current_count_change == -1) { + subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else { + return; } + info_->request_publisher_matched_count_.store(subscriptions_.size()); + } private: - - CustomClientInfo* info_; - std::set subscriptions_; + CustomClientInfo * info_; + std::set subscriptions_; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_CLIENT_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp index 45d631a69..e6622bbd6 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp @@ -31,42 +31,40 @@ class EventListenerInterface { public: + virtual eprosima::fastdds::dds::StatusCondition & get_statuscondition() const = 0; - virtual eprosima::fastdds::dds::StatusCondition& get_statuscondition() const = 0; + /// Take ready data for an event type. + /** + * \param event_type The event type to get data for. + * \param event_info A preallocated event information (from rmw/types.h) to fill with data + * \return `true` if data was successfully taken. + * \return `false` if data was not available, in this case nothing was written to event_info. + */ + virtual bool take_event( + rmw_event_type_t event_type, + void * event_info) = 0; - /// Take ready data for an event type. - /** - * \param event_type The event type to get data for. - * \param event_info A preallocated event information (from rmw/types.h) to fill with data - * \return `true` if data was successfully taken. - * \return `false` if data was not available, in this case nothing was written to event_info. - */ - virtual bool take_event( - rmw_event_type_t event_type, - void* event_info) = 0; + // Provide handlers to perform an action when a + // new event from this listener has ocurred + virtual void set_on_new_event_callback( + rmw_event_type_t event_type, + const void * user_data, + rmw_event_callback_t callback) = 0; - // Provide handlers to perform an action when a - // new event from this listener has ocurred - virtual void set_on_new_event_callback( - rmw_event_type_t event_type, - const void* user_data, - rmw_event_callback_t callback) = 0; - - eprosima::fastdds::dds::GuardCondition event_guard[RMW_EVENT_INVALID]; + eprosima::fastdds::dds::GuardCondition event_guard[RMW_EVENT_INVALID]; protected: + rmw_event_callback_t on_new_event_cb_[RMW_EVENT_INVALID] = {nullptr}; - rmw_event_callback_t on_new_event_cb_[RMW_EVENT_INVALID] = {nullptr}; - - const void* user_data_[RMW_EVENT_INVALID] = {nullptr}; + const void * user_data_[RMW_EVENT_INVALID] = {nullptr}; - std::mutex on_new_event_m_; + std::mutex on_new_event_m_; }; struct CustomEventInfo { - virtual EventListenerInterface* get_listener() const = 0; + virtual EventListenerInterface * get_listener() const = 0; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_EVENT_INFO_HPP_ 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 bb344d733..3e7282fa1 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 @@ -40,114 +40,109 @@ class PubListener; typedef struct CustomPublisherInfo : public CustomEventInfo { - virtual ~CustomPublisherInfo() = default; - - eprosima::fastdds::dds::DataWriter* data_writer_{nullptr}; - PubListener* listener_{nullptr}; - eprosima::fastdds::dds::TypeSupport type_support_; - const void* type_support_impl_{nullptr}; - rmw_gid_t publisher_gid{}; - const char* typesupport_identifier_{nullptr}; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - EventListenerInterface* - get_listener() const final; + virtual ~CustomPublisherInfo() = default; + + eprosima::fastdds::dds::DataWriter * data_writer_{nullptr}; + PubListener * listener_{nullptr}; + eprosima::fastdds::dds::TypeSupport type_support_; + const void * type_support_impl_{nullptr}; + rmw_gid_t publisher_gid{}; + const char * typesupport_identifier_{nullptr}; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + EventListenerInterface * + get_listener() const final; } CustomPublisherInfo; class PubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataWriterListener { public: - - explicit PubListener( - CustomPublisherInfo* info) - : publisher_info_(info) - , deadline_changes_(false) - , liveliness_changes_(false) - , incompatible_qos_changes_(false) - { - } - - // DataWriterListener implementation - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_publication_matched( - eprosima::fastdds::dds::DataWriter* /* writer */, - const eprosima::fastdds::dds::PublicationMatchedStatus& info) final - { - std::lock_guard lock(discovery_m_); - if (info.current_count_change == 1) - { - subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } - else if (info.current_count_change == -1) - { - subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } - } - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_offered_deadline_missed( - eprosima::fastdds::dds::DataWriter* writer, - const eprosima::fastdds::dds::OfferedDeadlineMissedStatus& status) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_liveliness_lost( - eprosima::fastdds::dds::DataWriter* writer, - const eprosima::fastdds::dds::LivelinessLostStatus& status) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_offered_incompatible_qos( - eprosima::fastdds::dds::DataWriter*, - const eprosima::fastdds::dds::OfferedIncompatibleQosStatus&) final; - - // PubListener API - size_t subscriptionCount() - { - std::lock_guard lock(discovery_m_); - return subscriptions_.size(); + explicit PubListener( + CustomPublisherInfo * info) + : publisher_info_(info) + , deadline_changes_(false) + , liveliness_changes_(false) + , incompatible_qos_changes_(false) + { + } + + // DataWriterListener implementation + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_publication_matched( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus & info) final + { + std::lock_guard lock(discovery_m_); + if (info.current_count_change == 1) { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else if (info.current_count_change == -1) { + subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); } - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool take_event( - rmw_event_type_t event_type, - void* event_info) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void set_on_new_event_callback( - rmw_event_type_t event_type, - const void* user_data, - rmw_event_callback_t callback) final; + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_offered_deadline_missed( + eprosima::fastdds::dds::DataWriter * writer, + const eprosima::fastdds::dds::OfferedDeadlineMissedStatus & status) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_liveliness_lost( + eprosima::fastdds::dds::DataWriter * writer, + const eprosima::fastdds::dds::LivelinessLostStatus & status) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_offered_incompatible_qos( + eprosima::fastdds::dds::DataWriter *, + const eprosima::fastdds::dds::OfferedIncompatibleQosStatus &) final; + + // PubListener API + size_t subscriptionCount() + { + std::lock_guard lock(discovery_m_); + return subscriptions_.size(); + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + eprosima::fastdds::dds::StatusCondition & get_statuscondition() const final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool take_event( + rmw_event_type_t event_type, + void * event_info) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void set_on_new_event_callback( + rmw_event_type_t event_type, + const void * user_data, + rmw_event_callback_t callback) final; private: + CustomPublisherInfo * publisher_info_ = nullptr; - CustomPublisherInfo* publisher_info_ = nullptr; - - std::set subscriptions_ - RCPPUTILS_TSA_GUARDED_BY( - discovery_m_); + std::set subscriptions_ + RCPPUTILS_TSA_GUARDED_BY( + discovery_m_); - std::mutex discovery_m_; + std::mutex discovery_m_; - bool deadline_changes_; - eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + bool deadline_changes_; + eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ + RCPPUTILS_TSA_GUARDED_BY( + on_new_event_m_); - bool liveliness_changes_; - eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + bool liveliness_changes_; + eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ + RCPPUTILS_TSA_GUARDED_BY( + on_new_event_m_); - bool incompatible_qos_changes_; - eprosima::fastdds::dds::OfferedIncompatibleQosStatus incompatible_qos_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + bool incompatible_qos_changes_; + eprosima::fastdds::dds::OfferedIncompatibleQosStatus incompatible_qos_status_ + RCPPUTILS_TSA_GUARDED_BY( + on_new_event_m_); }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index c269e4214..9093f7ec1 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -48,258 +48,241 @@ class ServicePubListener; enum class client_present_t { - FAILURE, // an error occurred when checking - MAYBE, // reader not matched, writer still present - YES, // reader matched - GONE // neither reader nor writer + FAILURE, // an error occurred when checking + MAYBE, // reader not matched, writer still present + YES, // reader matched + GONE // neither reader nor writer }; typedef struct CustomServiceInfo { - eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; - const void* request_type_support_impl_{nullptr}; - eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; - const void* response_type_support_impl_{nullptr}; - eprosima::fastdds::dds::DataReader* request_reader_{nullptr}; - eprosima::fastdds::dds::DataWriter* response_writer_{nullptr}; + eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; + const void * request_type_support_impl_{nullptr}; + eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; + const void * response_type_support_impl_{nullptr}; + eprosima::fastdds::dds::DataReader * request_reader_{nullptr}; + eprosima::fastdds::dds::DataWriter * response_writer_{nullptr}; - ServiceListener* listener_{nullptr}; - ServicePubListener* pub_listener_{nullptr}; + ServiceListener * listener_{nullptr}; + ServicePubListener * pub_listener_{nullptr}; - const char* typesupport_identifier_{nullptr}; + const char * typesupport_identifier_{nullptr}; } CustomServiceInfo; typedef struct CustomServiceRequest { - eprosima::fastrtps::rtps::SampleIdentity sample_identity_; - eprosima::fastcdr::FastBuffer* buffer_; - eprosima::fastdds::dds::SampleInfo sample_info_ {}; - - CustomServiceRequest() - : buffer_(nullptr) - { - } - + eprosima::fastrtps::rtps::SampleIdentity sample_identity_; + eprosima::fastcdr::FastBuffer * buffer_; + eprosima::fastdds::dds::SampleInfo sample_info_ {}; + + CustomServiceRequest() + : buffer_(nullptr) + { + } } CustomServiceRequest; class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener { - using subscriptions_set_t = - std::unordered_set; - using clients_endpoints_map_t = - std::unordered_map; + using subscriptions_set_t = + std::unordered_set; + using clients_endpoints_map_t = + std::unordered_map; public: - - explicit ServicePubListener( - CustomServiceInfo* info) - { - (void) info; - } - - void - on_publication_matched( - eprosima::fastdds::dds::DataWriter* /* writer */, - const eprosima::fastdds::dds::PublicationMatchedStatus& info) final - { - std::lock_guard lock(mutex_); - if (info.current_count_change == 1) - { - subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); - } - else if (info.current_count_change == -1) - { - eprosima::fastrtps::rtps::GUID_t erase_endpoint_guid = - eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle); - subscriptions_.erase(erase_endpoint_guid); - auto endpoint = clients_endpoints_.find(erase_endpoint_guid); - if (endpoint != clients_endpoints_.end()) - { - clients_endpoints_.erase(endpoint->second); - clients_endpoints_.erase(erase_endpoint_guid); - } - } - else - { - return; - } - cv_.notify_all(); + explicit ServicePubListener( + CustomServiceInfo * info) + { + (void) info; + } + + void + on_publication_matched( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus & info) final + { + std::lock_guard lock(mutex_); + if (info.current_count_change == 1) { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else if (info.current_count_change == -1) { + eprosima::fastrtps::rtps::GUID_t erase_endpoint_guid = + eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle); + subscriptions_.erase(erase_endpoint_guid); + auto endpoint = clients_endpoints_.find(erase_endpoint_guid); + if (endpoint != clients_endpoints_.end()) { + clients_endpoints_.erase(endpoint->second); + clients_endpoints_.erase(erase_endpoint_guid); + } + } else { + return; } - - template - bool - wait_for_subscription( - const eprosima::fastrtps::rtps::GUID_t& guid, - const std::chrono::duration& rel_time) + cv_.notify_all(); + } + + template + bool + wait_for_subscription( + const eprosima::fastrtps::rtps::GUID_t & guid, + const std::chrono::duration & rel_time) + { + auto guid_is_present = [this, guid]() RCPPUTILS_TSA_REQUIRES(mutex_)->bool { - auto guid_is_present = [this, guid]() RCPPUTILS_TSA_REQUIRES(mutex_)->bool - { - return subscriptions_.find(guid) != subscriptions_.end(); - }; + return subscriptions_.find(guid) != subscriptions_.end(); + }; - std::unique_lock lock(mutex_); - return cv_.wait_for(lock, rel_time, guid_is_present); - } + std::unique_lock lock(mutex_); + return cv_.wait_for(lock, rel_time, guid_is_present); + } - client_present_t - check_for_subscription( - const eprosima::fastrtps::rtps::GUID_t& guid) + client_present_t + check_for_subscription( + const eprosima::fastrtps::rtps::GUID_t & guid) + { { - { - std::lock_guard lock(mutex_); - // Check if the guid is still in the map - if (clients_endpoints_.find(guid) == clients_endpoints_.end()) - { - // Client is gone - return client_present_t::GONE; - } - } - // Wait for subscription - if (!wait_for_subscription(guid, std::chrono::milliseconds(100))) - { - return client_present_t::MAYBE; - } - return client_present_t::YES; + std::lock_guard lock(mutex_); + // Check if the guid is still in the map + if (clients_endpoints_.find(guid) == clients_endpoints_.end()) { + // Client is gone + return client_present_t::GONE; + } } - - void endpoint_erase_if_exists( - const eprosima::fastrtps::rtps::GUID_t& endpointGuid) - { - std::lock_guard lock(mutex_); - auto endpoint = clients_endpoints_.find(endpointGuid); - if (endpoint != clients_endpoints_.end()) - { - clients_endpoints_.erase(endpoint->second); - clients_endpoints_.erase(endpointGuid); - } + // Wait for subscription + if (!wait_for_subscription(guid, std::chrono::milliseconds(100))) { + return client_present_t::MAYBE; } - - void endpoint_add_reader_and_writer( - const eprosima::fastrtps::rtps::GUID_t& readerGuid, - const eprosima::fastrtps::rtps::GUID_t& writerGuid) - { - std::lock_guard lock(mutex_); - clients_endpoints_.emplace(readerGuid, writerGuid); - clients_endpoints_.emplace(writerGuid, readerGuid); + return client_present_t::YES; + } + + void endpoint_erase_if_exists( + const eprosima::fastrtps::rtps::GUID_t & endpointGuid) + { + std::lock_guard lock(mutex_); + auto endpoint = clients_endpoints_.find(endpointGuid); + if (endpoint != clients_endpoints_.end()) { + clients_endpoints_.erase(endpoint->second); + clients_endpoints_.erase(endpointGuid); } + } -private: + void endpoint_add_reader_and_writer( + const eprosima::fastrtps::rtps::GUID_t & readerGuid, + const eprosima::fastrtps::rtps::GUID_t & writerGuid) + { + std::lock_guard lock(mutex_); + clients_endpoints_.emplace(readerGuid, writerGuid); + clients_endpoints_.emplace(writerGuid, readerGuid); + } - std::mutex mutex_; - subscriptions_set_t subscriptions_ RCPPUTILS_TSA_GUARDED_BY( - mutex_); - clients_endpoints_map_t clients_endpoints_ RCPPUTILS_TSA_GUARDED_BY( - mutex_); - std::condition_variable cv_; +private: + std::mutex mutex_; + subscriptions_set_t subscriptions_ RCPPUTILS_TSA_GUARDED_BY( + mutex_); + clients_endpoints_map_t clients_endpoints_ RCPPUTILS_TSA_GUARDED_BY( + mutex_); + std::condition_variable cv_; }; class ServiceListener : public eprosima::fastdds::dds::DataReaderListener { public: - - explicit ServiceListener( - CustomServiceInfo* info) - : info_(info) - { + explicit ServiceListener( + CustomServiceInfo * info) + : info_(info) + { + } + + void + on_subscription_matched( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final + { + if (info.current_count_change == -1) { + info_->pub_listener_->endpoint_erase_if_exists( + eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); } + } - void - on_subscription_matched( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) final - { - if (info.current_count_change == -1) - { - info_->pub_listener_->endpoint_erase_if_exists( - eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } - } + size_t get_unread_resquests() + { + size_t ret_value = 0; + eprosima::fastdds::dds::LoanableSequence data_seq; + eprosima::fastdds::dds::SampleInfoSeq info_seq; - size_t get_unread_resquests() + if (ReturnCode_t::RETCODE_OK == info_->request_reader_->read( + data_seq, info_seq, + eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) { - size_t ret_value = 0; - eprosima::fastdds::dds::LoanableSequence data_seq; - eprosima::fastdds::dds::SampleInfoSeq info_seq; - - if (ReturnCode_t::RETCODE_OK == info_->request_reader_->read(data_seq, info_seq, - eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) - { - for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; count < info_seq.length(); ++count) - { - if (info_seq[count].valid_data) - { - ++ret_value; - } - } - - info_->request_reader_->return_loan(data_seq, info_seq); + for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; + count < info_seq.length(); ++count) + { + if (info_seq[count].valid_data) { + ++ret_value; } + } - return ret_value; - } - - void - on_data_available( - eprosima::fastdds::dds::DataReader*) final - { - std::unique_lock lock_mutex(on_new_request_m_); - - auto unread_requests = get_unread_resquests(); - - if (0 < unread_requests) - { - on_new_request_cb_(user_data_, unread_requests); - } + info_->request_reader_->return_loan(data_seq, info_seq); } - // Provide handlers to perform an action when a - // new event from this listener has ocurred - void - set_on_new_request_callback( - const void* user_data, - rmw_event_callback_t callback) - { - std::unique_lock lock_mutex(on_new_request_m_); - - if (callback) - { - auto unread_requests = get_unread_resquests(); + return ret_value; + } - if (0 < unread_requests) - { - callback(user_data_, unread_requests); - } + void + on_data_available( + eprosima::fastdds::dds::DataReader *) final + { + std::unique_lock lock_mutex(on_new_request_m_); - user_data_ = user_data; - on_new_request_cb_ = callback; + auto unread_requests = get_unread_resquests(); - eprosima::fastdds::dds::StatusMask status_mask = info_->request_reader_->get_status_mask(); - info_->request_reader_->set_listener(this, - status_mask << eprosima::fastdds::dds::StatusMask::data_available()); - } - else - { - eprosima::fastdds::dds::StatusMask status_mask = info_->request_reader_->get_status_mask(); - info_->request_reader_->set_listener(this, - status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); - - user_data_ = nullptr; - on_new_request_cb_ = nullptr; - } + if (0 < unread_requests) { + on_new_request_cb_(user_data_, unread_requests); } + } + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + void + set_on_new_request_callback( + const void * user_data, + rmw_event_callback_t callback) + { + std::unique_lock lock_mutex(on_new_request_m_); + + if (callback) { + auto unread_requests = get_unread_resquests(); + + if (0 < unread_requests) { + callback(user_data_, unread_requests); + } + + user_data_ = user_data; + on_new_request_cb_ = callback; + + eprosima::fastdds::dds::StatusMask status_mask = info_->request_reader_->get_status_mask(); + info_->request_reader_->set_listener( + this, + status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + } else { + eprosima::fastdds::dds::StatusMask status_mask = info_->request_reader_->get_status_mask(); + info_->request_reader_->set_listener( + this, + status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); + + user_data_ = nullptr; + on_new_request_cb_ = nullptr; + } + } private: + CustomServiceInfo * info_; - CustomServiceInfo* info_; - - rmw_event_callback_t on_new_request_cb_{nullptr}; + rmw_event_callback_t on_new_request_cb_{nullptr}; - const void* user_data_{nullptr}; + const void * user_data_{nullptr}; - std::mutex on_new_request_m_; + std::mutex on_new_request_m_; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ 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 e372294fb..3945c3fe2 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 @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -46,162 +47,158 @@ class SubListener; -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ struct LoanManager; } // namespace rmw_fastrtps_shared_cpp struct CustomSubscriberInfo : public CustomEventInfo { - virtual ~CustomSubscriberInfo() = default; - - eprosima::fastdds::dds::DataReader* data_reader_ {nullptr}; - SubListener* listener_{nullptr}; - eprosima::fastdds::dds::TypeSupport type_support_; - const void* type_support_impl_{nullptr}; - rmw_gid_t subscription_gid_{}; - const char* typesupport_identifier_{nullptr}; - std::shared_ptr loan_manager_; - - // for re-create or delete content filtered topic - const rmw_node_t* node_ {nullptr}; - rmw_dds_common::Context* common_context_ {nullptr}; - eprosima::fastdds::dds::DomainParticipant* dds_participant_ {nullptr}; - eprosima::fastdds::dds::Subscriber* subscriber_ {nullptr}; - std::string topic_name_mangled_; - eprosima::fastdds::dds::TopicDescription* topic_ {nullptr}; - eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic_ {nullptr}; - eprosima::fastdds::dds::DataReaderQos datareader_qos_; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - EventListenerInterface* - get_listener() const final; + virtual ~CustomSubscriberInfo() = default; + + eprosima::fastdds::dds::DataReader * data_reader_ {nullptr}; + SubListener * listener_{nullptr}; + eprosima::fastdds::dds::TypeSupport type_support_; + const void * type_support_impl_{nullptr}; + rmw_gid_t subscription_gid_{}; + const char * typesupport_identifier_{nullptr}; + std::shared_ptr loan_manager_; + + // for re-create or delete content filtered topic + const rmw_node_t * node_ {nullptr}; + rmw_dds_common::Context * common_context_ {nullptr}; + eprosima::fastdds::dds::DomainParticipant * dds_participant_ {nullptr}; + eprosima::fastdds::dds::Subscriber * subscriber_ {nullptr}; + std::string topic_name_mangled_; + eprosima::fastdds::dds::TopicDescription * topic_ {nullptr}; + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic_ {nullptr}; + eprosima::fastdds::dds::DataReaderQos datareader_qos_; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + EventListenerInterface * + get_listener() const final; }; class SubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataReaderListener { public: - - explicit SubListener( - CustomSubscriberInfo* info) - : subscriber_info_(info) - , deadline_changes_(false) - , liveliness_changes_(false) - , sample_lost_changes_(false) - , incompatible_qos_changes_(false) - { - } - - // DataReaderListener implementation - void - on_subscription_matched( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::SubscriptionMatchedStatus& info) final - { - { - std::lock_guard lock(discovery_m_); - if (info.current_count_change == 1) - { - publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } - else if (info.current_count_change == -1) - { - publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); - } - } - } - - void - on_data_available( - eprosima::fastdds::dds::DataReader* reader) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_requested_deadline_missed( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastrtps::RequestedDeadlineMissedStatus&) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_liveliness_changed( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastrtps::LivelinessChangedStatus&) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_sample_lost( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::SampleLostStatus&) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void - on_requested_incompatible_qos( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::RequestedIncompatibleQosStatus&) final; - - size_t publisherCount() + explicit SubListener( + CustomSubscriberInfo * info) + : subscriber_info_(info) + , deadline_changes_(false) + , liveliness_changes_(false) + , sample_lost_changes_(false) + , incompatible_qos_changes_(false) + { + } + + // DataReaderListener implementation + void + on_subscription_matched( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final + { { - std::lock_guard lock(discovery_m_); - return publishers_.size(); + std::lock_guard lock(discovery_m_); + if (info.current_count_change == 1) { + publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } else if (info.current_count_change == -1) { + publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } } - - // Provide handlers to perform an action when a - // new event from this listener has ocurred - void - set_on_new_message_callback( - const void* user_data, - rmw_event_callback_t callback); - - size_t get_unread_messages(); - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - eprosima::fastdds::dds::StatusCondition& get_statuscondition() const final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - bool take_event( - rmw_event_type_t event_type, - void* event_info) final; - - RMW_FASTRTPS_SHARED_CPP_PUBLIC - void set_on_new_event_callback( - rmw_event_type_t event_type, - const void* user_data, - rmw_event_callback_t callback) final; + } + + void + on_data_available( + eprosima::fastdds::dds::DataReader * reader) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_requested_deadline_missed( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastrtps::RequestedDeadlineMissedStatus &) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_liveliness_changed( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastrtps::LivelinessChangedStatus &) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_sample_lost( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::SampleLostStatus &) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_requested_incompatible_qos( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::RequestedIncompatibleQosStatus &) final; + + size_t publisherCount() + { + std::lock_guard lock(discovery_m_); + return publishers_.size(); + } + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + void + set_on_new_message_callback( + const void * user_data, + rmw_event_callback_t callback); + + size_t get_unread_messages(); + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + eprosima::fastdds::dds::StatusCondition & get_statuscondition() const final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool take_event( + rmw_event_type_t event_type, + void * event_info) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void set_on_new_event_callback( + rmw_event_type_t event_type, + const void * user_data, + rmw_event_callback_t callback) final; private: + CustomSubscriberInfo * subscriber_info_ = nullptr; - CustomSubscriberInfo* subscriber_info_ = nullptr; - - bool deadline_changes_; - eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + bool deadline_changes_; + eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ + RCPPUTILS_TSA_GUARDED_BY( + on_new_event_m_); - bool liveliness_changes_; - eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + bool liveliness_changes_; + eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ + RCPPUTILS_TSA_GUARDED_BY( + on_new_event_m_); - bool sample_lost_changes_; - eprosima::fastdds::dds::SampleLostStatus sample_lost_status_ - RCPPUTILS_TSA_GUARDED_BY( - on_new_event_m_); + bool sample_lost_changes_; + eprosima::fastdds::dds::SampleLostStatus sample_lost_status_ + RCPPUTILS_TSA_GUARDED_BY( + on_new_event_m_); - bool incompatible_qos_changes_; - eprosima::fastdds::dds::RequestedIncompatibleQosStatus incompatible_qos_status_ - RCPPUTILS_TSA_GUARDED_BY( - discovery_m_); + bool incompatible_qos_changes_; + eprosima::fastdds::dds::RequestedIncompatibleQosStatus incompatible_qos_status_ + RCPPUTILS_TSA_GUARDED_BY( + discovery_m_); - std::set publishers_ RCPPUTILS_TSA_GUARDED_BY( - internalMutex_); + std::set publishers_ RCPPUTILS_TSA_GUARDED_BY( + internalMutex_); - rmw_event_callback_t on_new_message_cb_{nullptr}; + rmw_event_callback_t on_new_message_cb_{nullptr}; - const void* new_message_user_data_{nullptr}; + const void * new_message_user_data_{nullptr}; - std::mutex on_new_message_m_; + std::mutex on_new_message_m_; - std::mutex discovery_m_; + std::mutex discovery_m_; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp index 997314de7..3801804b9 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_publisher_info.cpp @@ -20,207 +20,203 @@ #include "event_helpers.hpp" #include "types/event_types.hpp" -EventListenerInterface* +EventListenerInterface * CustomPublisherInfo::get_listener() const { - return listener_; + return listener_; } -eprosima::fastdds::dds::StatusCondition& PubListener::get_statuscondition() const +eprosima::fastdds::dds::StatusCondition & PubListener::get_statuscondition() const { - return publisher_info_->data_writer_->get_statuscondition(); + return publisher_info_->data_writer_->get_statuscondition(); } bool PubListener::take_event( - rmw_event_type_t event_type, - void* event_info) + rmw_event_type_t event_type, + void * event_info) { - assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); - - std::unique_lock lock_mutex(on_new_event_m_); - - switch (event_type){ - case RMW_EVENT_LIVELINESS_LOST: - { - auto rmw_data = static_cast(event_info); - if (liveliness_changes_) - { - rmw_data->total_count = liveliness_lost_status_.total_count; - rmw_data->total_count_change = liveliness_lost_status_.total_count_change; - liveliness_changes_ = false; - } - else - { - eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status; - publisher_info_->data_writer_->get_liveliness_lost_status(liveliness_lost_status); - rmw_data->total_count = liveliness_lost_status.total_count; - rmw_data->total_count_change = liveliness_lost_status.total_count_change; - } - liveliness_lost_status_.total_count_change = 0; + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + + std::unique_lock lock_mutex(on_new_event_m_); + + switch (event_type) { + case RMW_EVENT_LIVELINESS_LOST: + { + auto rmw_data = static_cast(event_info); + if (liveliness_changes_) { + rmw_data->total_count = liveliness_lost_status_.total_count; + rmw_data->total_count_change = liveliness_lost_status_.total_count_change; + liveliness_changes_ = false; + } else { + eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status; + publisher_info_->data_writer_->get_liveliness_lost_status(liveliness_lost_status); + rmw_data->total_count = liveliness_lost_status.total_count; + rmw_data->total_count_change = liveliness_lost_status.total_count_change; } - break; - case RMW_EVENT_OFFERED_DEADLINE_MISSED: - { - auto rmw_data = static_cast(event_info); - if (deadline_changes_) - { - rmw_data->total_count = offered_deadline_missed_status_.total_count; - rmw_data->total_count_change = offered_deadline_missed_status_.total_count_change; - deadline_changes_ = false; - } - else - { - eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status; - publisher_info_->data_writer_->get_offered_deadline_missed_status(offered_deadline_missed_status); - rmw_data->total_count = offered_deadline_missed_status.total_count; - rmw_data->total_count_change = offered_deadline_missed_status.total_count_change; - } - offered_deadline_missed_status_.total_count_change = 0; + liveliness_lost_status_.total_count_change = 0; + } + break; + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + { + auto rmw_data = static_cast(event_info); + if (deadline_changes_) { + rmw_data->total_count = offered_deadline_missed_status_.total_count; + rmw_data->total_count_change = offered_deadline_missed_status_.total_count_change; + deadline_changes_ = false; + } else { + eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status; + publisher_info_->data_writer_->get_offered_deadline_missed_status( + offered_deadline_missed_status); + rmw_data->total_count = offered_deadline_missed_status.total_count; + rmw_data->total_count_change = offered_deadline_missed_status.total_count_change; } - break; - case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - { - auto rmw_data = static_cast(event_info); - if (incompatible_qos_changes_) - { - rmw_data->total_count = incompatible_qos_status_.total_count; - rmw_data->total_count_change = incompatible_qos_status_.total_count_change; - rmw_data->last_policy_kind = - rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( - incompatible_qos_status_.last_policy_id); - incompatible_qos_changes_ = false; - } - else - { - eprosima::fastdds::dds::OfferedIncompatibleQosStatus offered_incompatible_qos_status; - publisher_info_->data_writer_->get_offered_incompatible_qos_status(offered_incompatible_qos_status); - rmw_data->total_count = offered_incompatible_qos_status.total_count; - rmw_data->total_count_change = offered_incompatible_qos_status.total_count_change; - rmw_data->last_policy_kind = - rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( - offered_incompatible_qos_status.last_policy_id); - } - incompatible_qos_status_.total_count_change = 0; + offered_deadline_missed_status_.total_count_change = 0; + } + break; + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + { + auto rmw_data = static_cast(event_info); + if (incompatible_qos_changes_) { + rmw_data->total_count = incompatible_qos_status_.total_count; + rmw_data->total_count_change = incompatible_qos_status_.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + incompatible_qos_status_.last_policy_id); + incompatible_qos_changes_ = false; + } else { + eprosima::fastdds::dds::OfferedIncompatibleQosStatus offered_incompatible_qos_status; + publisher_info_->data_writer_->get_offered_incompatible_qos_status( + offered_incompatible_qos_status); + rmw_data->total_count = offered_incompatible_qos_status.total_count; + rmw_data->total_count_change = offered_incompatible_qos_status.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + offered_incompatible_qos_status.last_policy_id); } - break; - default: - return false; - } - event_guard[event_type].set_trigger_value(false); - return true; + incompatible_qos_status_.total_count_change = 0; + } + break; + default: + return false; + } + event_guard[event_type].set_trigger_value(false); + return true; } void PubListener::set_on_new_event_callback( - rmw_event_type_t event_type, - const void* user_data, - rmw_event_callback_t callback) + rmw_event_type_t event_type, + const void * user_data, + rmw_event_callback_t callback) { - std::unique_lock lock_mutex(on_new_event_m_); - - if (callback) - { - switch (event_type) - { - case RMW_EVENT_LIVELINESS_LOST: - publisher_info_->data_writer_->get_liveliness_lost_status(liveliness_lost_status_); - callback(user_data, liveliness_lost_status_.total_count_change); - liveliness_lost_status_.total_count_change = 0; - break; - case RMW_EVENT_OFFERED_DEADLINE_MISSED: - publisher_info_->data_writer_->get_offered_deadline_missed_status(offered_deadline_missed_status_); - callback(user_data, - offered_deadline_missed_status_.total_count_change); - offered_deadline_missed_status_.total_count_change = 0; - break; - case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - publisher_info_->data_writer_->get_offered_incompatible_qos_status(incompatible_qos_status_); - callback(user_data, - incompatible_qos_status_.total_count_change); - incompatible_qos_status_.total_count_change = 0; - break; - default: - break; - } - - user_data_[event_type] = user_data; - on_new_event_cb_[event_type] = callback; - - eprosima::fastdds::dds::StatusMask status_mask = publisher_info_->data_writer_->get_status_mask(); - publisher_info_->data_writer_->set_listener(this, status_mask << rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event_type)); - } - else - { - eprosima::fastdds::dds::StatusMask status_mask = publisher_info_->data_writer_->get_status_mask(); - publisher_info_->data_writer_->set_listener(this, status_mask >> rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event_type)); - - user_data_[event_type] = nullptr; - on_new_event_cb_[event_type] = nullptr; + std::unique_lock lock_mutex(on_new_event_m_); + + if (callback) { + switch (event_type) { + case RMW_EVENT_LIVELINESS_LOST: + publisher_info_->data_writer_->get_liveliness_lost_status(liveliness_lost_status_); + callback(user_data, liveliness_lost_status_.total_count_change); + liveliness_lost_status_.total_count_change = 0; + break; + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + publisher_info_->data_writer_->get_offered_deadline_missed_status( + offered_deadline_missed_status_); + callback( + user_data, + offered_deadline_missed_status_.total_count_change); + offered_deadline_missed_status_.total_count_change = 0; + break; + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + publisher_info_->data_writer_->get_offered_incompatible_qos_status( + incompatible_qos_status_); + callback( + user_data, + incompatible_qos_status_.total_count_change); + incompatible_qos_status_.total_count_change = 0; + break; + default: + break; } + + user_data_[event_type] = user_data; + on_new_event_cb_[event_type] = callback; + + eprosima::fastdds::dds::StatusMask status_mask = + publisher_info_->data_writer_->get_status_mask(); + publisher_info_->data_writer_->set_listener( + this, status_mask << rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event_type)); + } else { + eprosima::fastdds::dds::StatusMask status_mask = + publisher_info_->data_writer_->get_status_mask(); + publisher_info_->data_writer_->set_listener( + this, status_mask >> rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event_type)); + + user_data_[event_type] = nullptr; + on_new_event_cb_[event_type] = nullptr; + } } void PubListener::on_offered_deadline_missed( - eprosima::fastdds::dds::DataWriter* /* writer */, - const eprosima::fastdds::dds::OfferedDeadlineMissedStatus& status) + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::OfferedDeadlineMissedStatus & status) { - std::unique_lock lock_mutex(on_new_event_m_); + std::unique_lock lock_mutex(on_new_event_m_); - // Assign absolute values - offered_deadline_missed_status_.total_count = status.total_count; - // Accumulate deltas - offered_deadline_missed_status_.total_count_change += status.total_count_change; + // Assign absolute values + offered_deadline_missed_status_.total_count = status.total_count; + // Accumulate deltas + offered_deadline_missed_status_.total_count_change += status.total_count_change; - deadline_changes_ = true; + deadline_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED]) - { - on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED](user_data_[RMW_EVENT_OFFERED_DEADLINE_MISSED], 1); - } + if (on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED]) { + on_new_event_cb_[RMW_EVENT_OFFERED_DEADLINE_MISSED](user_data_[RMW_EVENT_OFFERED_DEADLINE_MISSED + ], 1); + } - event_guard[RMW_EVENT_OFFERED_DEADLINE_MISSED].set_trigger_value(true); + event_guard[RMW_EVENT_OFFERED_DEADLINE_MISSED].set_trigger_value(true); } void PubListener::on_liveliness_lost( - eprosima::fastdds::dds::DataWriter* /* writer */, - const eprosima::fastdds::dds::LivelinessLostStatus& status) + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::LivelinessLostStatus & status) { - std::unique_lock lock_mutex(on_new_event_m_); + std::unique_lock lock_mutex(on_new_event_m_); - // Assign absolute values - liveliness_lost_status_.total_count = status.total_count; - // Accumulate deltas - liveliness_lost_status_.total_count_change += status.total_count_change; + // Assign absolute values + liveliness_lost_status_.total_count = status.total_count; + // Accumulate deltas + liveliness_lost_status_.total_count_change += status.total_count_change; - liveliness_changes_ = true; + liveliness_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST]) - { - on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST](user_data_[RMW_EVENT_LIVELINESS_LOST], 1); - } + if (on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST]) { + on_new_event_cb_[RMW_EVENT_LIVELINESS_LOST](user_data_[RMW_EVENT_LIVELINESS_LOST], 1); + } - event_guard[RMW_EVENT_LIVELINESS_LOST].set_trigger_value(true); + event_guard[RMW_EVENT_LIVELINESS_LOST].set_trigger_value(true); } void PubListener::on_offered_incompatible_qos( - eprosima::fastdds::dds::DataWriter* /* writer */, - const eprosima::fastdds::dds::OfferedIncompatibleQosStatus& status) + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::OfferedIncompatibleQosStatus & status) { - std::unique_lock lock_mutex(on_new_event_m_); + std::unique_lock lock_mutex(on_new_event_m_); - // Assign absolute values - incompatible_qos_status_.last_policy_id = status.last_policy_id; - incompatible_qos_status_.total_count = status.total_count; - // Accumulate deltas - incompatible_qos_status_.total_count_change += status.total_count_change; + // Assign absolute values + incompatible_qos_status_.last_policy_id = status.last_policy_id; + incompatible_qos_status_.total_count = status.total_count; + // Accumulate deltas + incompatible_qos_status_.total_count_change += status.total_count_change; - incompatible_qos_changes_ = true; + incompatible_qos_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE]) - { - on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE](user_data_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE], 1); - } + if (on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE]) { + on_new_event_cb_[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE](user_data_[ + RMW_EVENT_OFFERED_QOS_INCOMPATIBLE], 1); + } - event_guard[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE].set_trigger_value(true); + event_guard[RMW_EVENT_OFFERED_QOS_INCOMPATIBLE].set_trigger_value(true); } diff --git a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp index b67982abe..fa1b15acc 100644 --- a/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/custom_subscriber_info.cpp @@ -20,345 +20,339 @@ #include "event_helpers.hpp" #include "types/event_types.hpp" -EventListenerInterface* +EventListenerInterface * CustomSubscriberInfo::get_listener() const { - return listener_; + return listener_; } -eprosima::fastdds::dds::StatusCondition& SubListener::get_statuscondition() const +eprosima::fastdds::dds::StatusCondition & SubListener::get_statuscondition() const { - return subscriber_info_->data_reader_->get_statuscondition(); + return subscriber_info_->data_reader_->get_statuscondition(); } bool SubListener::take_event( - rmw_event_type_t event_type, - void* event_info) + rmw_event_type_t event_type, + void * event_info) { - assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + + std::unique_lock lock_mutex(on_new_event_m_); + + switch (event_type) { + case RMW_EVENT_LIVELINESS_CHANGED: + { + auto rmw_data = static_cast(event_info); + if (liveliness_changes_) { + rmw_data->alive_count = liveliness_changed_status_.alive_count; + rmw_data->not_alive_count = liveliness_changed_status_.not_alive_count; + rmw_data->alive_count_change = liveliness_changed_status_.alive_count_change; + rmw_data->not_alive_count_change = liveliness_changed_status_.not_alive_count_change; + liveliness_changes_ = false; + } else { + eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status; + subscriber_info_->data_reader_->get_liveliness_changed_status(liveliness_changed_status); + rmw_data->alive_count = liveliness_changed_status.alive_count; + rmw_data->not_alive_count = liveliness_changed_status.not_alive_count; + rmw_data->alive_count_change = liveliness_changed_status.alive_count_change; + rmw_data->not_alive_count_change = liveliness_changed_status.not_alive_count_change; + } + liveliness_changed_status_.alive_count_change = 0; + liveliness_changed_status_.not_alive_count_change = 0; + } + break; + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + { + auto rmw_data = static_cast(event_info); + if (deadline_changes_) { + rmw_data->total_count = requested_deadline_missed_status_.total_count; + rmw_data->total_count_change = requested_deadline_missed_status_.total_count_change; + deadline_changes_ = false; + } else { + eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status; + subscriber_info_->data_reader_->get_requested_deadline_missed_status( + requested_deadline_missed_status); + rmw_data->total_count = requested_deadline_missed_status.total_count; + rmw_data->total_count_change = requested_deadline_missed_status.total_count_change; + } + requested_deadline_missed_status_.total_count_change = 0; + } + break; + case RMW_EVENT_MESSAGE_LOST: + { + auto rmw_data = static_cast(event_info); + if (sample_lost_changes_) { + rmw_data->total_count = sample_lost_status_.total_count; + rmw_data->total_count_change = sample_lost_status_.total_count_change; + sample_lost_changes_ = false; + } else { + eprosima::fastdds::dds::SampleLostStatus sample_lost_status; + subscriber_info_->data_reader_->get_sample_lost_status(sample_lost_status); + rmw_data->total_count = sample_lost_status.total_count; + rmw_data->total_count_change = sample_lost_status.total_count_change; + } + sample_lost_status_.total_count_change = 0; + } + break; + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + { + auto rmw_data = static_cast(event_info); + if (incompatible_qos_changes_) { + rmw_data->total_count = incompatible_qos_status_.total_count; + rmw_data->total_count_change = incompatible_qos_status_.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + incompatible_qos_status_.last_policy_id); + incompatible_qos_changes_ = false; + } else { + eprosima::fastdds::dds::RequestedIncompatibleQosStatus + requested_qos_incompatible_qos_status; + subscriber_info_->data_reader_->get_requested_incompatible_qos_status( + requested_qos_incompatible_qos_status); + rmw_data->total_count = requested_qos_incompatible_qos_status.total_count; + rmw_data->total_count_change = requested_qos_incompatible_qos_status.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + requested_qos_incompatible_qos_status.last_policy_id); + } + incompatible_qos_status_.total_count_change = 0; + } + break; + default: + return false; + } + + event_guard[event_type].set_trigger_value(false); + return true; +} - std::unique_lock lock_mutex(on_new_event_m_); +void SubListener::set_on_new_event_callback( + rmw_event_type_t event_type, + const void * user_data, + rmw_event_callback_t callback) +{ + std::unique_lock lock_mutex(on_new_event_m_); - switch (event_type){ - case RMW_EVENT_LIVELINESS_CHANGED: + if (callback) { + switch (event_type) { + case RMW_EVENT_LIVELINESS_CHANGED: { - auto rmw_data = static_cast(event_info); - if (liveliness_changes_) - { - rmw_data->alive_count = liveliness_changed_status_.alive_count; - rmw_data->not_alive_count = liveliness_changed_status_.not_alive_count; - rmw_data->alive_count_change = liveliness_changed_status_.alive_count_change; - rmw_data->not_alive_count_change = liveliness_changed_status_.not_alive_count_change; - liveliness_changes_ = false; - } - else - { - eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status; - subscriber_info_->data_reader_->get_liveliness_changed_status(liveliness_changed_status); - rmw_data->alive_count = liveliness_changed_status.alive_count; - rmw_data->not_alive_count = liveliness_changed_status.not_alive_count; - rmw_data->alive_count_change = liveliness_changed_status.alive_count_change; - rmw_data->not_alive_count_change = liveliness_changed_status.not_alive_count_change; - } - liveliness_changed_status_.alive_count_change = 0; - liveliness_changed_status_.not_alive_count_change = 0; + subscriber_info_->data_reader_->get_liveliness_changed_status(liveliness_changed_status_); + callback( + user_data, liveliness_changed_status_.alive_count_change + + liveliness_changed_status_.not_alive_count_change); + liveliness_changed_status_.alive_count_change = 0; + liveliness_changed_status_.not_alive_count_change = 0; } break; - case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: { - auto rmw_data = static_cast(event_info); - if (deadline_changes_) - { - rmw_data->total_count = requested_deadline_missed_status_.total_count; - rmw_data->total_count_change = requested_deadline_missed_status_.total_count_change; - deadline_changes_ = false; - } - else - { - eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status; - subscriber_info_->data_reader_->get_requested_deadline_missed_status(requested_deadline_missed_status); - rmw_data->total_count = requested_deadline_missed_status.total_count; - rmw_data->total_count_change = requested_deadline_missed_status.total_count_change; - } - requested_deadline_missed_status_.total_count_change = 0; + subscriber_info_->data_reader_->get_requested_deadline_missed_status( + requested_deadline_missed_status_); + callback( + user_data, + requested_deadline_missed_status_.total_count_change); + requested_deadline_missed_status_.total_count_change = 0; } break; - case RMW_EVENT_MESSAGE_LOST: + case RMW_EVENT_MESSAGE_LOST: { - auto rmw_data = static_cast(event_info); - if (sample_lost_changes_) - { - rmw_data->total_count = sample_lost_status_.total_count; - rmw_data->total_count_change = sample_lost_status_.total_count_change; - sample_lost_changes_ = false; - } - else - { - eprosima::fastdds::dds::SampleLostStatus sample_lost_status; - subscriber_info_->data_reader_->get_sample_lost_status(sample_lost_status); - rmw_data->total_count = sample_lost_status.total_count; - rmw_data->total_count_change = sample_lost_status.total_count_change; - } - sample_lost_status_.total_count_change = 0; + subscriber_info_->data_reader_->get_sample_lost_status(sample_lost_status_); + callback( + user_data, + sample_lost_status_.total_count_change); + sample_lost_status_.total_count_change = 0; } break; - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: { - auto rmw_data = static_cast(event_info); - if (incompatible_qos_changes_) - { - rmw_data->total_count = incompatible_qos_status_.total_count; - rmw_data->total_count_change = incompatible_qos_status_.total_count_change; - rmw_data->last_policy_kind = - rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( - incompatible_qos_status_.last_policy_id); - incompatible_qos_changes_ = false; - } - else - { - eprosima::fastdds::dds::RequestedIncompatibleQosStatus requested_qos_incompatible_qos_status; - subscriber_info_->data_reader_->get_requested_incompatible_qos_status( - requested_qos_incompatible_qos_status); - rmw_data->total_count = requested_qos_incompatible_qos_status.total_count; - rmw_data->total_count_change = requested_qos_incompatible_qos_status.total_count_change; - rmw_data->last_policy_kind = - rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( - requested_qos_incompatible_qos_status.last_policy_id); - } - incompatible_qos_status_.total_count_change = 0; + subscriber_info_->data_reader_->get_requested_incompatible_qos_status( + incompatible_qos_status_); + callback( + user_data, + incompatible_qos_status_.total_count_change); + incompatible_qos_status_.total_count_change = 0; } break; - default: - return false; - } - - event_guard[event_type].set_trigger_value(false); - return true; -} - -void SubListener::set_on_new_event_callback( - rmw_event_type_t event_type, - const void* user_data, - rmw_event_callback_t callback) -{ - std::unique_lock lock_mutex(on_new_event_m_); - - if (callback) - { - switch (event_type){ - case RMW_EVENT_LIVELINESS_CHANGED: - { - subscriber_info_->data_reader_->get_liveliness_changed_status(liveliness_changed_status_); - callback(user_data, liveliness_changed_status_.alive_count_change + - liveliness_changed_status_.not_alive_count_change); - liveliness_changed_status_.alive_count_change = 0; - liveliness_changed_status_.not_alive_count_change = 0; - } - break; - case RMW_EVENT_REQUESTED_DEADLINE_MISSED: - { - subscriber_info_->data_reader_->get_requested_deadline_missed_status(requested_deadline_missed_status_); - callback(user_data, - requested_deadline_missed_status_.total_count_change); - requested_deadline_missed_status_.total_count_change = 0; - } - break; - case RMW_EVENT_MESSAGE_LOST: - { - subscriber_info_->data_reader_->get_sample_lost_status(sample_lost_status_); - callback(user_data, - sample_lost_status_.total_count_change); - sample_lost_status_.total_count_change = 0; - } - break; - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: - { - subscriber_info_->data_reader_->get_requested_incompatible_qos_status(incompatible_qos_status_); - callback(user_data, - incompatible_qos_status_.total_count_change); - incompatible_qos_status_.total_count_change = 0; - } - break; - default: - break; - } - - user_data_[event_type] = user_data; - on_new_event_cb_[event_type] = callback; - - eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); - subscriber_info_->data_reader_->set_listener(this, status_mask << rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event_type)); + default: + break; } - else - { - eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); - subscriber_info_->data_reader_->set_listener(this, status_mask >> rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event_type)); - user_data_[event_type] = nullptr; - on_new_event_cb_[event_type] = nullptr; - } + user_data_[event_type] = user_data; + on_new_event_cb_[event_type] = callback; + + eprosima::fastdds::dds::StatusMask status_mask = + subscriber_info_->data_reader_->get_status_mask(); + subscriber_info_->data_reader_->set_listener( + this, status_mask << rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event_type)); + } else { + eprosima::fastdds::dds::StatusMask status_mask = + subscriber_info_->data_reader_->get_status_mask(); + subscriber_info_->data_reader_->set_listener( + this, status_mask >> rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event_type)); + + user_data_[event_type] = nullptr; + on_new_event_cb_[event_type] = nullptr; + } } void SubListener::set_on_new_message_callback( - const void* user_data, - rmw_event_callback_t callback) + const void * user_data, + rmw_event_callback_t callback) { - std::unique_lock lock_mutex(on_new_message_m_); - - if (callback) - { - auto unread_messages = get_unread_messages(); - - if (0 < unread_messages) - { - callback(new_message_user_data_, unread_messages); - } + std::unique_lock lock_mutex(on_new_message_m_); - new_message_user_data_ = user_data; - on_new_message_cb_ = callback; + if (callback) { + auto unread_messages = get_unread_messages(); - eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); - subscriber_info_->data_reader_->set_listener(this, - status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + if (0 < unread_messages) { + callback(new_message_user_data_, unread_messages); } - else - { - eprosima::fastdds::dds::StatusMask status_mask = subscriber_info_->data_reader_->get_status_mask(); - subscriber_info_->data_reader_->set_listener(this, - status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); - new_message_user_data_ = nullptr; - on_new_message_cb_ = nullptr; - } + new_message_user_data_ = user_data; + on_new_message_cb_ = callback; + + eprosima::fastdds::dds::StatusMask status_mask = + subscriber_info_->data_reader_->get_status_mask(); + subscriber_info_->data_reader_->set_listener( + this, + status_mask << eprosima::fastdds::dds::StatusMask::data_available()); + } else { + eprosima::fastdds::dds::StatusMask status_mask = + subscriber_info_->data_reader_->get_status_mask(); + subscriber_info_->data_reader_->set_listener( + this, + status_mask >> eprosima::fastdds::dds::StatusMask::data_available()); + + new_message_user_data_ = nullptr; + on_new_message_cb_ = nullptr; + } } size_t SubListener::get_unread_messages() { - size_t ret_value = 0; - eprosima::fastdds::dds::LoanableSequence data_seq; - eprosima::fastdds::dds::SampleInfoSeq info_seq; - - if (ReturnCode_t::RETCODE_OK == subscriber_info_->data_reader_->read(data_seq, info_seq, - eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) + size_t ret_value = 0; + eprosima::fastdds::dds::LoanableSequence data_seq; + eprosima::fastdds::dds::SampleInfoSeq info_seq; + + if (ReturnCode_t::RETCODE_OK == subscriber_info_->data_reader_->read( + data_seq, info_seq, + eprosima::fastdds::dds::LENGTH_UNLIMITED, eprosima::fastdds::dds::NOT_READ_SAMPLE_STATE)) + { + for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; count < info_seq.length(); + ++count) { - for (eprosima::fastdds::dds::LoanableCollection::size_type count = 0; count < info_seq.length(); ++count) - { - if (info_seq[count].valid_data) - { - ++ret_value; - } - } - - subscriber_info_->data_reader_->return_loan(data_seq, info_seq); + if (info_seq[count].valid_data) { + ++ret_value; + } } - return ret_value; + subscriber_info_->data_reader_->return_loan(data_seq, info_seq); + } + + return ret_value; } void SubListener::on_data_available( - eprosima::fastdds::dds::DataReader*) + eprosima::fastdds::dds::DataReader *) { - std::unique_lock lock_mutex(on_new_message_m_); + std::unique_lock lock_mutex(on_new_message_m_); - if (on_new_message_cb_) - { - auto unread_messages = get_unread_messages(); + if (on_new_message_cb_) { + auto unread_messages = get_unread_messages(); - if (0 < unread_messages) - { - on_new_message_cb_(new_message_user_data_, unread_messages); - } + if (0 < unread_messages) { + on_new_message_cb_(new_message_user_data_, unread_messages); } + } } void SubListener::on_requested_deadline_missed( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::RequestedDeadlineMissedStatus& status) + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::RequestedDeadlineMissedStatus & status) { - std::unique_lock lock_mutex(on_new_event_m_); + std::unique_lock lock_mutex(on_new_event_m_); - // Assign absolute values - requested_deadline_missed_status_.total_count = status.total_count; - // Accumulate deltas - requested_deadline_missed_status_.total_count_change += status.total_count_change; + // Assign absolute values + requested_deadline_missed_status_.total_count = status.total_count; + // Accumulate deltas + requested_deadline_missed_status_.total_count_change += status.total_count_change; - deadline_changes_ = true; + deadline_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_REQUESTED_DEADLINE_MISSED]) - { - on_new_event_cb_[RMW_EVENT_REQUESTED_DEADLINE_MISSED](user_data_[RMW_EVENT_REQUESTED_DEADLINE_MISSED], 1); - } + if (on_new_event_cb_[RMW_EVENT_REQUESTED_DEADLINE_MISSED]) { + on_new_event_cb_[RMW_EVENT_REQUESTED_DEADLINE_MISSED](user_data_[ + RMW_EVENT_REQUESTED_DEADLINE_MISSED], 1); + } - event_guard[RMW_EVENT_REQUESTED_DEADLINE_MISSED].set_trigger_value(true); + event_guard[RMW_EVENT_REQUESTED_DEADLINE_MISSED].set_trigger_value(true); } void SubListener::on_liveliness_changed( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::LivelinessChangedStatus& status) + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::LivelinessChangedStatus & status) { - std::unique_lock lock_mutex(on_new_event_m_); + std::unique_lock lock_mutex(on_new_event_m_); - // Assign absolute values - liveliness_changed_status_.alive_count = status.alive_count; - liveliness_changed_status_.not_alive_count = status.not_alive_count; - // Accumulate deltas - liveliness_changed_status_.alive_count_change += status.alive_count_change; - liveliness_changed_status_.not_alive_count_change += status.not_alive_count_change; + // Assign absolute values + liveliness_changed_status_.alive_count = status.alive_count; + liveliness_changed_status_.not_alive_count = status.not_alive_count; + // Accumulate deltas + liveliness_changed_status_.alive_count_change += status.alive_count_change; + liveliness_changed_status_.not_alive_count_change += status.not_alive_count_change; - liveliness_changes_ = true; + liveliness_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_LIVELINESS_CHANGED]) - { - on_new_event_cb_[RMW_EVENT_LIVELINESS_CHANGED](user_data_[RMW_EVENT_LIVELINESS_CHANGED], 1); - } + if (on_new_event_cb_[RMW_EVENT_LIVELINESS_CHANGED]) { + on_new_event_cb_[RMW_EVENT_LIVELINESS_CHANGED](user_data_[RMW_EVENT_LIVELINESS_CHANGED], 1); + } - event_guard[RMW_EVENT_LIVELINESS_CHANGED].set_trigger_value(true); + event_guard[RMW_EVENT_LIVELINESS_CHANGED].set_trigger_value(true); } void SubListener::on_sample_lost( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::SampleLostStatus& status) + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::SampleLostStatus & status) { - std::lock_guard lock_mutex(on_new_event_m_); + std::lock_guard lock_mutex(on_new_event_m_); - // Assign absolute values - sample_lost_status_.total_count = status.total_count; - // Accumulate deltas - sample_lost_status_.total_count_change += status.total_count_change; + // Assign absolute values + sample_lost_status_.total_count = status.total_count; + // Accumulate deltas + sample_lost_status_.total_count_change += status.total_count_change; - sample_lost_changes_ = true; + sample_lost_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_MESSAGE_LOST]) - { - on_new_event_cb_[RMW_EVENT_MESSAGE_LOST](user_data_[RMW_EVENT_MESSAGE_LOST], 1); - } + if (on_new_event_cb_[RMW_EVENT_MESSAGE_LOST]) { + on_new_event_cb_[RMW_EVENT_MESSAGE_LOST](user_data_[RMW_EVENT_MESSAGE_LOST], 1); + } - event_guard[RMW_EVENT_MESSAGE_LOST].set_trigger_value(true); + event_guard[RMW_EVENT_MESSAGE_LOST].set_trigger_value(true); } void SubListener::on_requested_incompatible_qos( - eprosima::fastdds::dds::DataReader*, - const eprosima::fastdds::dds::RequestedIncompatibleQosStatus& status) + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::RequestedIncompatibleQosStatus & status) { - std::lock_guard lock_mutex(on_new_event_m_); + std::lock_guard lock_mutex(on_new_event_m_); - // Assign absolute values - incompatible_qos_status_.last_policy_id = status.last_policy_id; - incompatible_qos_status_.total_count = status.total_count; - // Accumulate deltas - incompatible_qos_status_.total_count_change += status.total_count_change; + // Assign absolute values + incompatible_qos_status_.last_policy_id = status.last_policy_id; + incompatible_qos_status_.total_count = status.total_count; + // Accumulate deltas + incompatible_qos_status_.total_count_change += status.total_count_change; - incompatible_qos_changes_ = true; + incompatible_qos_changes_ = true; - if (on_new_event_cb_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE]) - { - on_new_event_cb_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE](user_data_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE], 1); - } + if (on_new_event_cb_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE]) { + on_new_event_cb_[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE](user_data_[ + RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE], 1); + } - event_guard[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE].set_trigger_value(true); + event_guard[RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE].set_trigger_value(true); } diff --git a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp index 93a2c4957..917a100a4 100644 --- a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp @@ -32,87 +32,79 @@ rmw_ret_t rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count( - rmw_context_t* context) + rmw_context_t * context) { - std::lock_guard guard(context->impl->mutex); - - assert(context); - assert(context->impl); - assert(0u < context->impl->count); - - if (--context->impl->count > 0) - { - return RMW_RET_OK; - } - - rmw_ret_t err = RMW_RET_OK; - rmw_ret_t ret = RMW_RET_OK; - rmw_error_string_t error_string; - - ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); - if (RMW_RET_OK != ret) - { - return ret; - } - - auto common_context = static_cast(context->impl->common); - auto participant_info = static_cast(context->impl->participant_info); - - if (!common_context->graph_cache.remove_participant(common_context->gid)) - { - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " - "couldn't remove Participant gid from graph_cache when destroying Participant"); - } - - ret = rmw_fastrtps_shared_cpp::destroy_subscription( - context->implementation_identifier, - participant_info, - common_context->sub); - // Try to clean the other objects if the above failed. - if (RMW_RET_OK != ret) - { - error_string = rmw_get_error_string(); - rmw_reset_error(); - } - err = rmw_fastrtps_shared_cpp::destroy_publisher( - context->implementation_identifier, - participant_info, - common_context->pub); - if (RMW_RET_OK != ret && RMW_RET_OK != err) - { - // We can just return one error, log about the previous one. - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) - ": 'destroy_subscription' failed\n"); - ret = err; - error_string = rmw_get_error_string(); - rmw_reset_error(); - } - err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); - if (RMW_RET_OK != ret && RMW_RET_OK != err) - { - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) - ": 'destroy_publisher' failed\n"); - ret = err; - } - else if (RMW_RET_OK != ret) - { - RMW_SET_ERROR_MSG(error_string.str); - } - - common_context->graph_cache.clear_on_change_callback(); - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( - common_context->graph_guard_condition)) - { - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " - "couldn't destroy graph_guard_condtion"); - } - - delete common_context; - context->impl->common = nullptr; - context->impl->participant_info = nullptr; + std::lock_guard guard(context->impl->mutex); + + assert(context); + assert(context->impl); + assert(0u < context->impl->count); + + if (--context->impl->count > 0) { + return RMW_RET_OK; + } + + rmw_ret_t err = RMW_RET_OK; + rmw_ret_t ret = RMW_RET_OK; + rmw_error_string_t error_string; + + ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); + if (RMW_RET_OK != ret) { return ret; + } + + auto common_context = static_cast(context->impl->common); + auto participant_info = static_cast(context->impl->participant_info); + + if (!common_context->graph_cache.remove_participant(common_context->gid)) { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " + "couldn't remove Participant gid from graph_cache when destroying Participant"); + } + + ret = rmw_fastrtps_shared_cpp::destroy_subscription( + context->implementation_identifier, + participant_info, + common_context->sub); + // Try to clean the other objects if the above failed. + if (RMW_RET_OK != ret) { + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + err = rmw_fastrtps_shared_cpp::destroy_publisher( + context->implementation_identifier, + participant_info, + common_context->pub); + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + // We can just return one error, log about the previous one. + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_subscription' failed\n"); + ret = err; + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_publisher' failed\n"); + ret = err; + } else if (RMW_RET_OK != ret) { + RMW_SET_ERROR_MSG(error_string.str); + } + + common_context->graph_cache.clear_on_change_callback(); + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->graph_guard_condition)) + { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " + "couldn't destroy graph_guard_condtion"); + } + + delete common_context; + context->impl->common = nullptr; + context->impl->participant_info = nullptr; + return ret; } diff --git a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp index 01da1b159..2b6b6b002 100644 --- a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp +++ b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp @@ -35,174 +35,153 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" -using rmw_dds_common::operator <<; +using rmw_dds_common::operator<<; static void node_listener( - rmw_context_t* context); + rmw_context_t * context); rmw_ret_t rmw_fastrtps_shared_cpp::run_listener_thread( - rmw_context_t* context) + rmw_context_t * context) { - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); - auto common_context = static_cast(context->impl->common); - common_context->thread_is_running.store(true); - common_context->listener_thread_gc = rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( - context->implementation_identifier); - if (common_context->listener_thread_gc) - { - try - { - common_context->listener_thread = std::thread(node_listener, context); - return RMW_RET_OK; - } - catch (const std::exception& exc) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to create std::thread: %s", exc.what()); - } - catch (...) - { - RMW_SET_ERROR_MSG("Failed to create std::thread"); - } - } - else - { - RMW_SET_ERROR_MSG("Failed to create guard condition"); + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.store(true); + common_context->listener_thread_gc = rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( + context->implementation_identifier); + if (common_context->listener_thread_gc) { + try { + common_context->listener_thread = std::thread(node_listener, context); + return RMW_RET_OK; + } catch (const std::exception & exc) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to create std::thread: %s", exc.what()); + } catch (...) { + RMW_SET_ERROR_MSG("Failed to create std::thread"); } - common_context->thread_is_running.store(false); - if (common_context->listener_thread_gc) + } else { + RMW_SET_ERROR_MSG("Failed to create guard condition"); + } + common_context->thread_is_running.store(false); + if (common_context->listener_thread_gc) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->listener_thread_gc)) { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( - common_context->listener_thread_gc)) - { - RCUTILS_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" - RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); - } + RCUTILS_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" + RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); } - return RMW_RET_ERROR; + } + return RMW_RET_ERROR; } rmw_ret_t rmw_fastrtps_shared_cpp::join_listener_thread( - rmw_context_t* context) + rmw_context_t * context) { - auto common_context = static_cast(context->impl->common); - common_context->thread_is_running.exchange(false); - rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( - context->implementation_identifier, common_context->listener_thread_gc); - if (RMW_RET_OK != rmw_ret) - { - return rmw_ret; - } - try - { - common_context->listener_thread.join(); - } - catch (const std::exception& exc) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to join std::thread: %s", exc.what()); - return RMW_RET_ERROR; - } - catch (...) - { - RMW_SET_ERROR_MSG("Failed to join std::thread"); - return RMW_RET_ERROR; - } - rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( - common_context->listener_thread_gc); - if (RMW_RET_OK != rmw_ret) - { - return rmw_ret; - } - return RMW_RET_OK; + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.exchange(false); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + context->implementation_identifier, common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + try { + common_context->listener_thread.join(); + } catch (const std::exception & exc) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to join std::thread: %s", exc.what()); + return RMW_RET_ERROR; + } catch (...) { + RMW_SET_ERROR_MSG("Failed to join std::thread"); + return RMW_RET_ERROR; + } + rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + return RMW_RET_OK; } #define TERMINATE_THREAD(msg) \ - { \ - RCUTILS_SAFE_FWRITE_TO_STDERR( \ - RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ - RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) \ - ": ros discovery info listener thread will shutdown ...\n"); \ - break; \ - } + { \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ + RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) \ + ": ros discovery info listener thread will shutdown ...\n"); \ + break; \ + } void node_listener( - rmw_context_t* context) + rmw_context_t * context) { - assert(nullptr != context); - assert(nullptr != context->impl); - assert(nullptr != context->impl->common); - auto common_context = static_cast(context->impl->common); - while (common_context->thread_is_running.load()) + assert(nullptr != context); + assert(nullptr != context->impl); + assert(nullptr != context->impl->common); + auto common_context = static_cast(context->impl->common); + while (common_context->thread_is_running.load()) { + assert(nullptr != common_context->sub); + assert(nullptr != common_context->sub->data); + void * subscriptions_buffer[] = {common_context->sub->data}; + void * guard_conditions_buffer[] = {common_context->listener_thread_gc->data}; + rmw_subscriptions_t subscriptions; + rmw_guard_conditions_t guard_conditions; + subscriptions.subscriber_count = 1; + subscriptions.subscribers = subscriptions_buffer; + guard_conditions.guard_condition_count = 1; + guard_conditions.guard_conditions = guard_conditions_buffer; + // number of conditions of a subscription is 2 + rmw_wait_set_t * wait_set = rmw_fastrtps_shared_cpp::__rmw_create_wait_set( + context->implementation_identifier, context, 2); + if (nullptr == wait_set) { + TERMINATE_THREAD("failed to create wait set"); + } + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_wait( + context->implementation_identifier, + &subscriptions, + &guard_conditions, + nullptr, + nullptr, + nullptr, + wait_set, + nullptr)) { - assert(nullptr != common_context->sub); - assert(nullptr != common_context->sub->data); - void* subscriptions_buffer[] = {common_context->sub->data}; - void* guard_conditions_buffer[] = {common_context->listener_thread_gc->data}; - rmw_subscriptions_t subscriptions; - rmw_guard_conditions_t guard_conditions; - subscriptions.subscriber_count = 1; - subscriptions.subscribers = subscriptions_buffer; - guard_conditions.guard_condition_count = 1; - guard_conditions.guard_conditions = guard_conditions_buffer; - // number of conditions of a subscription is 2 - rmw_wait_set_t* wait_set = rmw_fastrtps_shared_cpp::__rmw_create_wait_set( - context->implementation_identifier, context, 2); - if (nullptr == wait_set) - { - TERMINATE_THREAD("failed to create wait set"); - } - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_wait( - context->implementation_identifier, - &subscriptions, - &guard_conditions, - nullptr, - nullptr, - nullptr, - wait_set, - nullptr)) - { - TERMINATE_THREAD("rmw_wait failed"); - } - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( - context->implementation_identifier, wait_set)) + TERMINATE_THREAD("rmw_wait failed"); + } + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( + context->implementation_identifier, wait_set)) + { + TERMINATE_THREAD("failed to destroy wait set"); + } + if (subscriptions_buffer[0]) { + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + bool taken = true; + + while (taken) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_take( + context->implementation_identifier, + common_context->sub, + static_cast(&msg), + &taken, + nullptr)) { - TERMINATE_THREAD("failed to destroy wait set"); + TERMINATE_THREAD("__rmw_take failed"); } - if (subscriptions_buffer[0]) - { - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - bool taken = true; - - while (taken) - { - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_take( - context->implementation_identifier, - common_context->sub, - static_cast(&msg), - &taken, - nullptr)) - { - TERMINATE_THREAD("__rmw_take failed"); - } - if (taken) - { - if (std::memcmp( - reinterpret_cast(common_context->gid.data), - reinterpret_cast(&msg.gid.data), - RMW_GID_STORAGE_SIZE) == 0) - { - // ignore local messages - continue; - } - common_context->graph_cache.update_participant_entities(msg); - } - } + if (taken) { + if (std::memcmp( + reinterpret_cast(common_context->gid.data), + reinterpret_cast(&msg.gid.data), + RMW_GID_STORAGE_SIZE) == 0) + { + // ignore local messages + continue; + } + common_context->graph_cache.update_participant_entities(msg); } + } } + } } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index 3269f4acb..efd071929 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -30,135 +30,132 @@ #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/utils.hpp" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ rmw_ret_t __rmw_destroy_client( - const char* identifier, - rmw_node_t* node, - rmw_client_t* client) + const char * identifier, + rmw_node_t * node, + rmw_client_t * client) { - rmw_ret_t final_ret = RMW_RET_OK; - auto common_context = static_cast(node->context->impl->common); - auto participant_info = - static_cast(node->context->impl->participant_info); - auto info = static_cast(client->data); - + rmw_ret_t final_ret = RMW_RET_OK; + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + auto info = static_cast(client->data); + + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->request_writer_->guid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->response_reader_->guid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( + gid, common_context->gid, node->name, node->namespace_); + final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + } + + auto show_previous_error = + [&final_ret]() { - // Update graph - std::lock_guard guard(common_context->node_update_mutex); - rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->request_writer_->guid()); - common_context->graph_cache.dissociate_writer( - gid, - common_context->gid, - node->name, - node->namespace_); - gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->response_reader_->guid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_reader( - gid, common_context->gid, node->name, node->namespace_); - final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( - identifier, - common_context->pub, - static_cast(&msg), - nullptr); + if (RMW_RET_OK != final_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + rmw_reset_error(); + } + }; + + ///// + // Delete DataWriter and DataReader + { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Keep pointers to topics, so we can remove them later + auto response_topic = info->response_reader_->get_topicdescription(); + auto request_topic = info->request_writer_->get_topic(); + + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_reader_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("destroy_client() failed to delete datareader"); + final_ret = RMW_RET_ERROR; + info->response_reader_->set_listener(nullptr); } - auto show_previous_error = - [&final_ret]() - { - if (RMW_RET_OK != final_ret) - { - RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); - RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); - rmw_reset_error(); - } - }; - - ///// - // Delete DataWriter and DataReader - { - std::lock_guard lck(participant_info->entity_creation_mutex_); - - // Keep pointers to topics, so we can remove them later - auto response_topic = info->response_reader_->get_topicdescription(); - auto request_topic = info->request_writer_->get_topic(); - - // Delete DataReader - ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_reader_); - if (ret != ReturnCode_t::RETCODE_OK) - { - show_previous_error(); - RMW_SET_ERROR_MSG("destroy_client() failed to delete datareader"); - final_ret = RMW_RET_ERROR; - info->response_reader_->set_listener(nullptr); - } - - // Delete DataWriter - ret = participant_info->publisher_->delete_datawriter(info->request_writer_); - if (ret != ReturnCode_t::RETCODE_OK) - { - show_previous_error(); - RMW_SET_ERROR_MSG("destroy_client() failed to delete datawriter"); - final_ret = RMW_RET_ERROR; - info->request_writer_->set_listener(nullptr); - } - - // Delete DataWriter listener - if (nullptr != info->pub_listener_) - { - delete info->pub_listener_; - } - - // Delete topics and unregister types - remove_topic_and_type(participant_info, request_topic, info->request_type_support_); - remove_topic_and_type(participant_info, response_topic, info->response_type_support_); - - // Delete CustomClientInfo structure - delete info; + // Delete DataWriter + ret = participant_info->publisher_->delete_datawriter(info->request_writer_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("destroy_client() failed to delete datawriter"); + final_ret = RMW_RET_ERROR; + info->request_writer_->set_listener(nullptr); } - rmw_free(const_cast(client->service_name)); - rmw_client_free(client); + // Delete DataWriter listener + if (nullptr != info->pub_listener_) { + delete info->pub_listener_; + } + + // Delete topics and unregister types + remove_topic_and_type(participant_info, request_topic, info->request_type_support_); + remove_topic_and_type(participant_info, response_topic, info->response_type_support_); + + // Delete CustomClientInfo structure + delete info; + } + + rmw_free(const_cast(client->service_name)); + rmw_client_free(client); - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion - return final_ret; + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return final_ret; } rmw_ret_t __rmw_client_request_publisher_get_actual_qos( - const rmw_client_t* client, - rmw_qos_profile_t* qos) + const rmw_client_t * client, + rmw_qos_profile_t * qos) { - auto cli = static_cast(client->data); - eprosima::fastdds::dds::DataWriter* fastdds_rw = cli->request_writer_; - dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); - return RMW_RET_OK; + auto cli = static_cast(client->data); + eprosima::fastdds::dds::DataWriter * fastdds_rw = cli->request_writer_; + dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); + return RMW_RET_OK; } rmw_ret_t __rmw_client_response_subscription_get_actual_qos( - const rmw_client_t* client, - rmw_qos_profile_t* qos) + const rmw_client_t * client, + rmw_qos_profile_t * qos) { - auto cli = static_cast(client->data); - eprosima::fastdds::dds::DataReader* fastdds_dr = cli->response_reader_; - dds_qos_to_rmw_qos(fastdds_dr->get_qos(), qos); - return RMW_RET_OK; + auto cli = static_cast(client->data); + eprosima::fastdds::dds::DataReader * fastdds_dr = cli->response_reader_; + dds_qos_to_rmw_qos(fastdds_dr->get_qos(), qos); + return RMW_RET_OK; } rmw_ret_t __rmw_client_set_on_new_response_callback( - rmw_client_t* rmw_client, - rmw_event_callback_t callback, - const void* user_data) + rmw_client_t * rmw_client, + rmw_event_callback_t callback, + const void * user_data) { - auto custom_client_info = static_cast(rmw_client->data); - custom_client_info->listener_->set_on_new_response_callback( - user_data, - callback); - return RMW_RET_OK; + auto custom_client_info = static_cast(rmw_client->data); + custom_client_info->listener_->set_on_new_response_callback( + user_data, + callback); + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp index b5d45b5e6..413791b6c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_event.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_event.cpp @@ -22,129 +22,130 @@ #include "types/event_types.hpp" static const std::unordered_set g_rmw_event_type_set{ - RMW_EVENT_LIVELINESS_CHANGED, - RMW_EVENT_REQUESTED_DEADLINE_MISSED, - RMW_EVENT_LIVELINESS_LOST, - RMW_EVENT_OFFERED_DEADLINE_MISSED, - RMW_EVENT_MESSAGE_LOST, - RMW_EVENT_OFFERED_QOS_INCOMPATIBLE, - RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE + RMW_EVENT_LIVELINESS_CHANGED, + RMW_EVENT_REQUESTED_DEADLINE_MISSED, + RMW_EVENT_LIVELINESS_LOST, + RMW_EVENT_OFFERED_DEADLINE_MISSED, + RMW_EVENT_MESSAGE_LOST, + RMW_EVENT_OFFERED_QOS_INCOMPATIBLE, + RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE }; -namespace rmw_fastrtps_shared_cpp { -namespace internal { +namespace rmw_fastrtps_shared_cpp +{ +namespace internal +{ eprosima::fastdds::dds::StatusMask rmw_event_to_dds_statusmask( - const rmw_event_type_t event_type) + const rmw_event_type_t event_type) { - eprosima::fastdds::dds::StatusMask ret_statusmask = eprosima::fastdds::dds::StatusMask::none(); - switch (event_type) - { - case RMW_EVENT_LIVELINESS_CHANGED: - ret_statusmask = eprosima::fastdds::dds::StatusMask::liveliness_changed(); - break; - case RMW_EVENT_REQUESTED_DEADLINE_MISSED: - ret_statusmask = eprosima::fastdds::dds::StatusMask::requested_deadline_missed(); - break; - case RMW_EVENT_LIVELINESS_LOST: - ret_statusmask = eprosima::fastdds::dds::StatusMask::liveliness_lost(); - break; - case RMW_EVENT_OFFERED_DEADLINE_MISSED: - ret_statusmask = eprosima::fastdds::dds::StatusMask::offered_deadline_missed(); - break; - case RMW_EVENT_MESSAGE_LOST: - ret_statusmask = eprosima::fastdds::dds::StatusMask::sample_lost(); - break; - case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - ret_statusmask = eprosima::fastdds::dds::StatusMask::offered_incompatible_qos(); - break; - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: - ret_statusmask = eprosima::fastdds::dds::StatusMask::requested_incompatible_qos(); - break; - default: - break; - } - - return ret_statusmask; + eprosima::fastdds::dds::StatusMask ret_statusmask = eprosima::fastdds::dds::StatusMask::none(); + switch (event_type) { + case RMW_EVENT_LIVELINESS_CHANGED: + ret_statusmask = eprosima::fastdds::dds::StatusMask::liveliness_changed(); + break; + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + ret_statusmask = eprosima::fastdds::dds::StatusMask::requested_deadline_missed(); + break; + case RMW_EVENT_LIVELINESS_LOST: + ret_statusmask = eprosima::fastdds::dds::StatusMask::liveliness_lost(); + break; + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + ret_statusmask = eprosima::fastdds::dds::StatusMask::offered_deadline_missed(); + break; + case RMW_EVENT_MESSAGE_LOST: + ret_statusmask = eprosima::fastdds::dds::StatusMask::sample_lost(); + break; + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + ret_statusmask = eprosima::fastdds::dds::StatusMask::offered_incompatible_qos(); + break; + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + ret_statusmask = eprosima::fastdds::dds::StatusMask::requested_incompatible_qos(); + break; + default: + break; + } + + return ret_statusmask; } bool is_event_supported( - rmw_event_type_t event_type) + rmw_event_type_t event_type) { - return g_rmw_event_type_set.count(event_type) == 1; + return g_rmw_event_type_set.count(event_type) == 1; } rmw_qos_policy_kind_t dds_qos_policy_to_rmw_qos_policy( - eprosima::fastdds::dds::QosPolicyId_t policy_id) + eprosima::fastdds::dds::QosPolicyId_t policy_id) { - using eprosima::fastdds::dds::QosPolicyId_t; - - switch (policy_id){ - case QosPolicyId_t::DURABILITY_QOS_POLICY_ID: - return RMW_QOS_POLICY_DURABILITY; - case QosPolicyId_t::DEADLINE_QOS_POLICY_ID: - return RMW_QOS_POLICY_DEADLINE; - case QosPolicyId_t::LIVELINESS_QOS_POLICY_ID: - return RMW_QOS_POLICY_LIVELINESS; - case QosPolicyId_t::RELIABILITY_QOS_POLICY_ID: - return RMW_QOS_POLICY_RELIABILITY; - case QosPolicyId_t::HISTORY_QOS_POLICY_ID: - return RMW_QOS_POLICY_HISTORY; - case QosPolicyId_t::LIFESPAN_QOS_POLICY_ID: - return RMW_QOS_POLICY_LIFESPAN; - default: - return RMW_QOS_POLICY_INVALID; - } + using eprosima::fastdds::dds::QosPolicyId_t; + + switch (policy_id) { + case QosPolicyId_t::DURABILITY_QOS_POLICY_ID: + return RMW_QOS_POLICY_DURABILITY; + case QosPolicyId_t::DEADLINE_QOS_POLICY_ID: + return RMW_QOS_POLICY_DEADLINE; + case QosPolicyId_t::LIVELINESS_QOS_POLICY_ID: + return RMW_QOS_POLICY_LIVELINESS; + case QosPolicyId_t::RELIABILITY_QOS_POLICY_ID: + return RMW_QOS_POLICY_RELIABILITY; + case QosPolicyId_t::HISTORY_QOS_POLICY_ID: + return RMW_QOS_POLICY_HISTORY; + case QosPolicyId_t::LIFESPAN_QOS_POLICY_ID: + return RMW_QOS_POLICY_LIFESPAN; + default: + return RMW_QOS_POLICY_INVALID; + } } } // namespace internal rmw_ret_t __rmw_init_event( - const char* identifier, - rmw_event_t* rmw_event, - const char* topic_endpoint_impl_identifier, - void* data, - rmw_event_type_t event_type) + const char * identifier, + rmw_event_t * rmw_event, + const char * topic_endpoint_impl_identifier, + void * data, + rmw_event_type_t event_type) { - RMW_CHECK_ARGUMENT_FOR_NULL(identifier, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(topic_endpoint_impl_identifier, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(data, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - topic endpoint, - topic_endpoint_impl_identifier, - identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (!internal::is_event_supported(event_type)) - { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("provided event_type is not supported by %s", identifier); - return RMW_RET_UNSUPPORTED; - } - - rmw_event->implementation_identifier = topic_endpoint_impl_identifier; - rmw_event->data = data; - rmw_event->event_type = event_type; - CustomEventInfo* event = static_cast(rmw_event->data); - eprosima::fastdds::dds::StatusMask statusmask = event->get_listener()->get_statuscondition().get_enabled_statuses(); - statusmask << internal::rmw_event_to_dds_statusmask(event_type); - event->get_listener()->get_statuscondition().set_enabled_statuses(statusmask); - - return RMW_RET_OK; + RMW_CHECK_ARGUMENT_FOR_NULL(identifier, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_endpoint_impl_identifier, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(data, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + topic endpoint, + topic_endpoint_impl_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!internal::is_event_supported(event_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("provided event_type is not supported by %s", identifier); + return RMW_RET_UNSUPPORTED; + } + + rmw_event->implementation_identifier = topic_endpoint_impl_identifier; + rmw_event->data = data; + rmw_event->event_type = event_type; + CustomEventInfo * event = static_cast(rmw_event->data); + eprosima::fastdds::dds::StatusMask statusmask = + event->get_listener()->get_statuscondition().get_enabled_statuses(); + statusmask << internal::rmw_event_to_dds_statusmask(event_type); + event->get_listener()->get_statuscondition().set_enabled_statuses(statusmask); + + return RMW_RET_OK; } rmw_ret_t __rmw_event_set_callback( - rmw_event_t* rmw_event, - rmw_event_callback_t callback, - const void* user_data) + rmw_event_t * rmw_event, + rmw_event_callback_t callback, + const void * user_data) { - auto custom_event_info = static_cast(rmw_event->data); - custom_event_info->get_listener()->set_on_new_event_callback( - rmw_event->event_type, - user_data, - callback); - return RMW_RET_OK; + auto custom_event_info = static_cast(rmw_event->data); + custom_event_info->get_listener()->set_on_new_event_callback( + rmw_event->event_type, + user_data, + callback); + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index b6625f1e6..cb8cb02c6 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -30,124 +30,120 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ rmw_ret_t __rmw_send_request( - const char* identifier, - const rmw_client_t* client, - const void* ros_request, - int64_t* sequence_id) + const char * identifier, + const rmw_client_t * client, + const void * ros_request, + int64_t * sequence_id) { - RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - client, - client->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_INVALID_ARGUMENT); - - rmw_ret_t returnedValue = RMW_RET_ERROR; - - auto info = static_cast(client->data); - assert(info); - - eprosima::fastrtps::rtps::WriteParams wparams; - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = false; - data.data = const_cast(ros_request); - data.impl = info->request_type_support_impl_; - wparams.related_sample_identity().writer_guid() = info->reader_guid_; - if (info->request_writer_->write(&data, wparams)) - { - returnedValue = RMW_RET_OK; - *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 | - wparams.sample_identity().sequence_number().low; - } - else - { - RMW_SET_ERROR_MSG("cannot publish data"); - } - - return returnedValue; + RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + client, + client->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_INVALID_ARGUMENT); + + rmw_ret_t returnedValue = RMW_RET_ERROR; + + auto info = static_cast(client->data); + assert(info); + + eprosima::fastrtps::rtps::WriteParams wparams; + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + data.data = const_cast(ros_request); + data.impl = info->request_type_support_impl_; + wparams.related_sample_identity().writer_guid() = info->reader_guid_; + if (info->request_writer_->write(&data, wparams)) { + returnedValue = RMW_RET_OK; + *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 | + wparams.sample_identity().sequence_number().low; + } else { + RMW_SET_ERROR_MSG("cannot publish data"); + } + + return returnedValue; } rmw_ret_t __rmw_take_request( - const char* identifier, - const rmw_service_t* service, - rmw_service_info_t* request_header, - void* ros_request, - bool* taken) + const char * identifier, + const rmw_service_t * service, + rmw_service_info_t * request_header, + void * ros_request, + bool * taken) { - RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, - service->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + service, + service->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - *taken = false; + *taken = false; - auto info = static_cast(service->data); - assert(info); + auto info = static_cast(service->data); + assert(info); - CustomServiceRequest request; + CustomServiceRequest request; - request.buffer_ = new eprosima::fastcdr::FastBuffer(); + request.buffer_ = new eprosima::fastcdr::FastBuffer(); - if (request.buffer_ != nullptr) + if (request.buffer_ != nullptr) { + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = request.buffer_; + data.impl = nullptr; // not used when is_cdr_buffer is true + if (info->request_reader_->take_next_sample( + &data, + &request.sample_info_) == ReturnCode_t::RETCODE_OK) { - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = true; - data.data = request.buffer_; - data.impl = nullptr; // not used when is_cdr_buffer is true - if (info->request_reader_->take_next_sample(&data, &request.sample_info_) == ReturnCode_t::RETCODE_OK) - { - if (request.sample_info_.valid_data) - { - request.sample_identity_ = request.sample_info_.sample_identity; - // Use response subscriber guid (on related_sample_identity) when present. - const eprosima::fastrtps::rtps::GUID_t& reader_guid = - request.sample_info_.related_sample_identity.writer_guid(); - if (reader_guid != eprosima::fastrtps::rtps::GUID_t::unknown()) - { - request.sample_identity_.writer_guid() = reader_guid; - } - - // Save both guids in the clients_endpoints map - const eprosima::fastrtps::rtps::GUID_t& writer_guid = - request.sample_info_.sample_identity.writer_guid(); - info->pub_listener_->endpoint_add_reader_and_writer(reader_guid, writer_guid); - - auto raw_type_support = dynamic_cast( - info->response_type_support_.get()); - eprosima::fastcdr::Cdr deser(*request.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, - eprosima::fastcdr::Cdr::DDS_CDR); - if (raw_type_support->deserializeROSmessage( - deser, ros_request, info->request_type_support_impl_)) - { - // Get header - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( - request.sample_identity_.writer_guid(), - request_header->request_id.writer_guid); - request_header->request_id.sequence_number = - ((int64_t)request.sample_identity_.sequence_number().high) << - 32 | request.sample_identity_.sequence_number().low; - request_header->source_timestamp = request.sample_info_.source_timestamp.to_ns(); - request_header->received_timestamp = request.sample_info_.source_timestamp.to_ns(); - *taken = true; - } - - } + if (request.sample_info_.valid_data) { + request.sample_identity_ = request.sample_info_.sample_identity; + // Use response subscriber guid (on related_sample_identity) when present. + const eprosima::fastrtps::rtps::GUID_t & reader_guid = + request.sample_info_.related_sample_identity.writer_guid(); + if (reader_guid != eprosima::fastrtps::rtps::GUID_t::unknown()) { + request.sample_identity_.writer_guid() = reader_guid; } - delete request.buffer_; + // Save both guids in the clients_endpoints map + const eprosima::fastrtps::rtps::GUID_t & writer_guid = + request.sample_info_.sample_identity.writer_guid(); + info->pub_listener_->endpoint_add_reader_and_writer(reader_guid, writer_guid); + + auto raw_type_support = dynamic_cast( + info->response_type_support_.get()); + eprosima::fastcdr::Cdr deser(*request.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + if (raw_type_support->deserializeROSmessage( + deser, ros_request, info->request_type_support_impl_)) + { + // Get header + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + request.sample_identity_.writer_guid(), + request_header->request_id.writer_guid); + request_header->request_id.sequence_number = + ((int64_t)request.sample_identity_.sequence_number().high) << + 32 | request.sample_identity_.sequence_number().low; + request_header->source_timestamp = request.sample_info_.source_timestamp.to_ns(); + request_header->received_timestamp = request.sample_info_.source_timestamp.to_ns(); + *taken = true; + } + } } + delete request.buffer_; + } + - return RMW_RET_OK; + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index 94b9a7dc2..371f5b640 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -28,142 +28,137 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ rmw_ret_t __rmw_take_response( - const char* identifier, - const rmw_client_t* client, - rmw_service_info_t* request_header, - void* ros_response, - bool* taken) + const char * identifier, + const rmw_client_t * client, + rmw_service_info_t * request_header, + void * ros_response, + bool * taken) { - RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - client, - client->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - - *taken = false; - - auto info = static_cast(client->data); - assert(info); - - CustomClientResponse response; - - // Todo(sloretz) eliminate heap allocation pending eprosima/Fast-CDR#19 - response.buffer_.reset(new eprosima::fastcdr::FastBuffer()); - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = true; - data.data = response.buffer_.get(); - data.impl = nullptr; // not used when is_cdr_buffer is true - if (info->response_reader_->take_next_sample(&data, &response.sample_info_) == ReturnCode_t::RETCODE_OK) - { - if (response.sample_info_.valid_data) + RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + client, + client->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + + *taken = false; + + auto info = static_cast(client->data); + assert(info); + + CustomClientResponse response; + + // Todo(sloretz) eliminate heap allocation pending eprosima/Fast-CDR#19 + response.buffer_.reset(new eprosima::fastcdr::FastBuffer()); + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = response.buffer_.get(); + data.impl = nullptr; // not used when is_cdr_buffer is true + if (info->response_reader_->take_next_sample( + &data, + &response.sample_info_) == ReturnCode_t::RETCODE_OK) + { + if (response.sample_info_.valid_data) { + response.sample_identity_ = response.sample_info_.related_sample_identity; + + if (response.sample_identity_.writer_guid() == info->reader_guid_ || + response.sample_identity_.writer_guid() == info->writer_guid_) + { + auto raw_type_support = dynamic_cast( + info->response_type_support_.get()); + eprosima::fastcdr::Cdr deser( + *response.buffer_, + eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + if (raw_type_support->deserializeROSmessage( + deser, ros_response, info->response_type_support_impl_)) { - response.sample_identity_ = response.sample_info_.related_sample_identity; - - if (response.sample_identity_.writer_guid() == info->reader_guid_ || - response.sample_identity_.writer_guid() == info->writer_guid_) - { - auto raw_type_support = dynamic_cast( - info->response_type_support_.get()); - eprosima::fastcdr::Cdr deser( - *response.buffer_, - eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, - eprosima::fastcdr::Cdr::DDS_CDR); - if (raw_type_support->deserializeROSmessage( - deser, ros_response, info->response_type_support_impl_)) - { - request_header->source_timestamp = response.sample_info_.source_timestamp.to_ns(); - request_header->received_timestamp = response.sample_info_.reception_timestamp.to_ns(); - request_header->request_id.sequence_number = - ((int64_t)response.sample_identity_.sequence_number().high) << - 32 | response.sample_identity_.sequence_number().low; - - *taken = true; - } - } + request_header->source_timestamp = response.sample_info_.source_timestamp.to_ns(); + request_header->received_timestamp = response.sample_info_.reception_timestamp.to_ns(); + request_header->request_id.sequence_number = + ((int64_t)response.sample_identity_.sequence_number().high) << + 32 | response.sample_identity_.sequence_number().low; + + *taken = true; } + } } + } - return RMW_RET_OK; + return RMW_RET_OK; } rmw_ret_t __rmw_send_response( - const char* identifier, - const rmw_service_t* service, - rmw_request_id_t* request_header, - void* ros_response) + const char * identifier, + const rmw_service_t * service, + rmw_request_id_t * request_header, + void * ros_response) { - RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - service, - service->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); - - rmw_ret_t returnedValue = RMW_RET_ERROR; - - auto info = static_cast(service->data); - assert(info); - - eprosima::fastrtps::rtps::WriteParams wparams; - rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastrtps_guid( - request_header->writer_guid, - &wparams.related_sample_identity().writer_guid()); - wparams.related_sample_identity().sequence_number().high = - (int32_t)((request_header->sequence_number & 0xFFFFFFFF00000000) >> 32); - wparams.related_sample_identity().sequence_number().low = - (int32_t)(request_header->sequence_number & 0xFFFFFFFF); - - // TODO(MiguelCompany) The following block is a workaround for the race on the - // discovery of services. It is (ab)using a related_sample_identity on the request - // with the GUID of the response reader, so we can wait here for it to be matched to - // the server response writer. In the future, this should be done with the mechanism - // explained on OMG DDS-RPC 1.0 spec under section 7.6.2 (Enhanced Service Mapping) - - // According to the list of possible entity kinds in section 9.3.1.2 of RTPS - // readers will have this bit on, while writers will not. We use this to know - // if the related guid is the request writer or the response reader. - constexpr uint8_t entity_id_is_reader_bit = 0x04; - const eprosima::fastrtps::rtps::GUID_t& related_guid = - wparams.related_sample_identity().writer_guid(); - if ((related_guid.entityId.value[3] & entity_id_is_reader_bit) != 0) - { - // Related guid is a reader, so it is the response subscription guid. - // Wait for the response writer to be matched with it. - auto listener = info->pub_listener_; - client_present_t ret = listener->check_for_subscription(related_guid); - if (ret == client_present_t::GONE) - { - return RMW_RET_OK; - } - else if (ret == client_present_t::MAYBE) - { - RMW_SET_ERROR_MSG("client will not receive response"); - return RMW_RET_TIMEOUT; - } - } - - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = false; - data.data = const_cast(ros_response); - data.impl = info->response_type_support_impl_; - if (info->response_writer_->write(&data, wparams)) - { - returnedValue = RMW_RET_OK; + RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + service, + service->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); + + rmw_ret_t returnedValue = RMW_RET_ERROR; + + auto info = static_cast(service->data); + assert(info); + + eprosima::fastrtps::rtps::WriteParams wparams; + rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastrtps_guid( + request_header->writer_guid, + &wparams.related_sample_identity().writer_guid()); + wparams.related_sample_identity().sequence_number().high = + (int32_t)((request_header->sequence_number & 0xFFFFFFFF00000000) >> 32); + wparams.related_sample_identity().sequence_number().low = + (int32_t)(request_header->sequence_number & 0xFFFFFFFF); + + // TODO(MiguelCompany) The following block is a workaround for the race on the + // discovery of services. It is (ab)using a related_sample_identity on the request + // with the GUID of the response reader, so we can wait here for it to be matched to + // the server response writer. In the future, this should be done with the mechanism + // explained on OMG DDS-RPC 1.0 spec under section 7.6.2 (Enhanced Service Mapping) + + // According to the list of possible entity kinds in section 9.3.1.2 of RTPS + // readers will have this bit on, while writers will not. We use this to know + // if the related guid is the request writer or the response reader. + constexpr uint8_t entity_id_is_reader_bit = 0x04; + const eprosima::fastrtps::rtps::GUID_t & related_guid = + wparams.related_sample_identity().writer_guid(); + if ((related_guid.entityId.value[3] & entity_id_is_reader_bit) != 0) { + // Related guid is a reader, so it is the response subscription guid. + // Wait for the response writer to be matched with it. + auto listener = info->pub_listener_; + client_present_t ret = listener->check_for_subscription(related_guid); + if (ret == client_present_t::GONE) { + return RMW_RET_OK; + } else if (ret == client_present_t::MAYBE) { + RMW_SET_ERROR_MSG("client will not receive response"); + return RMW_RET_TIMEOUT; } - else - { - RMW_SET_ERROR_MSG("cannot publish data"); - } - - return returnedValue; + } + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + data.data = const_cast(ros_response); + data.impl = info->response_type_support_impl_; + if (info->response_writer_->write(&data, wparams)) { + returnedValue = RMW_RET_OK; + } else { + RMW_SET_ERROR_MSG("cannot publish data"); + } + + return returnedValue; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index 5da0eeac1..7d27da250 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -42,133 +42,130 @@ #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" #include "rmw_fastrtps_shared_cpp/utils.hpp" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ rmw_ret_t __rmw_destroy_service( - const char* identifier, - rmw_node_t* node, - rmw_service_t* service) + const char * identifier, + rmw_node_t * node, + rmw_service_t * service) { - rmw_ret_t final_ret = RMW_RET_OK; - - auto common_context = static_cast(node->context->impl->common); - auto participant_info = - static_cast(node->context->impl->participant_info); - auto info = static_cast(service->data); + rmw_ret_t final_ret = RMW_RET_OK; + + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + auto info = static_cast(service->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->request_reader_->guid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->response_writer_->guid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_writer( + gid, common_context->gid, node->name, node->namespace_); + final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + } + + auto show_previous_error = + [&final_ret]() { - // Update graph - std::lock_guard guard(common_context->node_update_mutex); - rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->request_reader_->guid()); - common_context->graph_cache.dissociate_reader( - gid, - common_context->gid, - node->name, - node->namespace_); - gid = rmw_fastrtps_shared_cpp::create_rmw_gid( - identifier, info->response_writer_->guid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_writer( - gid, common_context->gid, node->name, node->namespace_); - final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( - identifier, - common_context->pub, - static_cast(&msg), - nullptr); + if (RMW_RET_OK != final_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + rmw_reset_error(); + } + }; + + ///// + // Delete DataWriter and DataReader + { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Keep pointers to topics, so we can remove them later + auto response_topic = info->response_writer_->get_topic(); + auto request_topic = info->request_reader_->get_topicdescription(); + + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_reader_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("Fail in delete datareader"); + final_ret = RMW_RET_ERROR; } - auto show_previous_error = - [&final_ret]() - { - if (RMW_RET_OK != final_ret) - { - RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); - RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); - rmw_reset_error(); - } - }; - - ///// - // Delete DataWriter and DataReader - { - std::lock_guard lck(participant_info->entity_creation_mutex_); - - // Keep pointers to topics, so we can remove them later - auto response_topic = info->response_writer_->get_topic(); - auto request_topic = info->request_reader_->get_topicdescription(); - - // Delete DataReader - ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_reader_); - if (ret != ReturnCode_t::RETCODE_OK) - { - show_previous_error(); - RMW_SET_ERROR_MSG("Fail in delete datareader"); - final_ret = RMW_RET_ERROR; - } - - // Delete DataWriter - ret = participant_info->publisher_->delete_datawriter(info->response_writer_); - if (ret != ReturnCode_t::RETCODE_OK) - { - show_previous_error(); - RMW_SET_ERROR_MSG("Fail in delete datawriter"); - final_ret = RMW_RET_ERROR; - } - - // Delete DataWriter listener - if (nullptr != info->pub_listener_) - { - delete info->pub_listener_; - } - - // Delete topics and unregister types - remove_topic_and_type(participant_info, request_topic, info->request_type_support_); - remove_topic_and_type(participant_info, response_topic, info->response_type_support_); - - // Delete CustomServiceInfo structure - delete info; + // Delete DataWriter + ret = participant_info->publisher_->delete_datawriter(info->response_writer_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("Fail in delete datawriter"); + final_ret = RMW_RET_ERROR; } - rmw_free(const_cast(service->service_name)); - rmw_service_free(service); + // Delete DataWriter listener + if (nullptr != info->pub_listener_) { + delete info->pub_listener_; + } + + // Delete topics and unregister types + remove_topic_and_type(participant_info, request_topic, info->request_type_support_); + remove_topic_and_type(participant_info, response_topic, info->response_type_support_); + + // Delete CustomServiceInfo structure + delete info; + } + + rmw_free(const_cast(service->service_name)); + rmw_service_free(service); - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion - return final_ret; + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return final_ret; } rmw_ret_t __rmw_service_response_publisher_get_actual_qos( - const rmw_service_t* service, - rmw_qos_profile_t* qos) + const rmw_service_t * service, + rmw_qos_profile_t * qos) { - auto srv = static_cast(service->data); - eprosima::fastdds::dds::DataWriter* fastdds_rw = srv->response_writer_; - dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); - return RMW_RET_OK; + auto srv = static_cast(service->data); + eprosima::fastdds::dds::DataWriter * fastdds_rw = srv->response_writer_; + dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); + return RMW_RET_OK; } rmw_ret_t __rmw_service_request_subscription_get_actual_qos( - const rmw_service_t* service, - rmw_qos_profile_t* qos) + const rmw_service_t * service, + rmw_qos_profile_t * qos) { - auto srv = static_cast(service->data); - eprosima::fastdds::dds::DataReader* fastdds_rr = srv->request_reader_; - dds_qos_to_rmw_qos(fastdds_rr->get_qos(), qos); - return RMW_RET_OK; + auto srv = static_cast(service->data); + eprosima::fastdds::dds::DataReader * fastdds_rr = srv->request_reader_; + dds_qos_to_rmw_qos(fastdds_rr->get_qos(), qos); + return RMW_RET_OK; } rmw_ret_t __rmw_service_set_on_new_request_callback( - rmw_service_t* rmw_service, - rmw_event_callback_t callback, - const void* user_data) + rmw_service_t * rmw_service, + rmw_event_callback_t callback, + const void * user_data) { - auto custom_service_info = static_cast(rmw_service->data); - custom_service_info->listener_->set_on_new_request_callback( - user_data, - callback); - return RMW_RET_OK; + auto custom_service_info = static_cast(rmw_service->data); + custom_service_info->listener_->set_on_new_request_callback( + user_data, + callback); + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 32765a26d..983a21ac2 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -35,260 +35,243 @@ #include "rmw_fastrtps_shared_cpp/utils.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ rmw_ret_t __rmw_destroy_subscription( - const char* identifier, - const rmw_node_t* node, - rmw_subscription_t* subscription, - bool reset_cft) + const char * identifier, + const rmw_node_t * node, + rmw_subscription_t * subscription, + bool reset_cft) { - assert(node->implementation_identifier == identifier); - assert(subscription->implementation_identifier == identifier); + 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); - { - // Update graph - std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_reader( - info->subscription_gid_, common_context->gid, node->name, node->namespace_); - ret = rmw_fastrtps_shared_cpp::__rmw_publish( - identifier, - common_context->pub, - static_cast(&msg), - nullptr); - if (RMW_RET_OK != ret) - { - error_state = *rmw_get_error_state(); - error_string = rmw_get_error_string(); - rmw_reset_error(); - } + 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); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + 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); - rmw_ret_t local_ret = destroy_subscription(identifier, participant_info, subscription, reset_cft); - 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; + auto participant_info = + static_cast(node->context->impl->participant_info); + rmw_ret_t local_ret = destroy_subscription(identifier, participant_info, subscription, reset_cft); + 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"); } - else if (RMW_RET_OK != ret) - { - rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); - } - return ret; + 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 __rmw_subscription_count_matched_publishers( - const rmw_subscription_t* subscription, - size_t* publisher_count) + const rmw_subscription_t * subscription, + size_t * publisher_count) { - auto info = static_cast(subscription->data); + auto info = static_cast(subscription->data); - *publisher_count = info->listener_->publisherCount(); + *publisher_count = info->listener_->publisherCount(); - return RMW_RET_OK; + return RMW_RET_OK; } rmw_ret_t __rmw_subscription_get_actual_qos( - const rmw_subscription_t* subscription, - rmw_qos_profile_t* qos) + const rmw_subscription_t * subscription, + rmw_qos_profile_t * qos) { - auto info = static_cast(subscription->data); - eprosima::fastdds::dds::DataReader* fastdds_dr = info->data_reader_; - const eprosima::fastdds::dds::DataReaderQos& dds_qos = fastdds_dr->get_qos(); + auto info = static_cast(subscription->data); + eprosima::fastdds::dds::DataReader * fastdds_dr = info->data_reader_; + const eprosima::fastdds::dds::DataReaderQos & dds_qos = fastdds_dr->get_qos(); - dds_qos_to_rmw_qos(dds_qos, qos); + dds_qos_to_rmw_qos(dds_qos, qos); - return RMW_RET_OK; + return RMW_RET_OK; } rmw_ret_t __rmw_subscription_set_content_filter( - rmw_subscription_t* subscription, - const rmw_subscription_content_filter_options_t* options - ) + rmw_subscription_t * subscription, + const rmw_subscription_content_filter_options_t * options +) { - auto info = static_cast(subscription->data); - eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic = info->filtered_topic_; - const bool filter_expression_empty = (*options->filter_expression == '\0'); + auto info = static_cast(subscription->data); + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = info->filtered_topic_; + const bool filter_expression_empty = (*options->filter_expression == '\0'); - if (!filtered_topic && filter_expression_empty) - { - // can't reset current subscriber - RMW_SET_ERROR_MSG( - "current subscriber has no content filter topic"); - return RMW_RET_ERROR; + if (!filtered_topic && filter_expression_empty) { + // can't reset current subscriber + RMW_SET_ERROR_MSG( + "current subscriber has no content filter topic"); + return RMW_RET_ERROR; + } else if (filtered_topic && !filter_expression_empty) { + std::vector expression_parameters; + for (size_t i = 0; i < options->expression_parameters.size; ++i) { + expression_parameters.push_back(options->expression_parameters.data[i]); } - else if (filtered_topic && !filter_expression_empty) - { - std::vector expression_parameters; - for (size_t i = 0; i < options->expression_parameters.size; ++i) - { - expression_parameters.push_back(options->expression_parameters.data[i]); - } - ReturnCode_t ret = - filtered_topic->set_filter_expression(options->filter_expression, expression_parameters); - if (ret != ReturnCode_t::RETCODE_OK) - { - RMW_SET_ERROR_MSG( - "failed to set_filter_expression"); - return RMW_RET_ERROR; - } - return RMW_RET_OK; + ReturnCode_t ret = + filtered_topic->set_filter_expression(options->filter_expression, expression_parameters); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG( + "failed to set_filter_expression"); + return RMW_RET_ERROR; } + return RMW_RET_OK; + } - eprosima::fastdds::dds::DomainParticipant* dds_participant = - info->dds_participant_; - eprosima::fastdds::dds::TopicDescription* des_topic = nullptr; - const char* eprosima_fastrtps_identifier = subscription->implementation_identifier; + eprosima::fastdds::dds::DomainParticipant * dds_participant = + info->dds_participant_; + eprosima::fastdds::dds::TopicDescription * des_topic = nullptr; + const char * eprosima_fastrtps_identifier = subscription->implementation_identifier; - rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( - eprosima_fastrtps_identifier, - info->node_, - subscription, - true /* reset_cft */); - if (ret != RMW_RET_OK) - { - RMW_SET_ERROR_MSG("delete subscription with reset cft"); - return RMW_RET_ERROR; - } + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + eprosima_fastrtps_identifier, + info->node_, + subscription, + true /* reset_cft */); + if (ret != RMW_RET_OK) { + RMW_SET_ERROR_MSG("delete subscription with reset cft"); + return RMW_RET_ERROR; + } - if (!filtered_topic) + if (!filtered_topic) { + // create content filtered topic + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( + dds_participant, info->topic_, + info->topic_name_mangled_, options, &filtered_topic)) { - // create content filtered topic - eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic = nullptr; - if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( - dds_participant, info->topic_, - info->topic_name_mangled_, options, &filtered_topic)) - { - RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); - return RMW_RET_ERROR; - } - info->filtered_topic_ = filtered_topic; - des_topic = filtered_topic; - } - else - { - // use the existing parent topic - des_topic = info->topic_; + RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); + return RMW_RET_ERROR; } + info->filtered_topic_ = filtered_topic; + des_topic = filtered_topic; + } else { + // use the existing parent topic + des_topic = info->topic_; + } - // create data reader - eprosima::fastdds::dds::Subscriber* subscriber = info->subscriber_; - const rmw_subscription_options_t* subscription_options = - &subscription->options; - if (!rmw_fastrtps_shared_cpp::create_datareader( - info->datareader_qos_, - subscription_options, - subscriber, - des_topic, - info->listener_, - &info->data_reader_)) + // create data reader + eprosima::fastdds::dds::Subscriber * subscriber = info->subscriber_; + const rmw_subscription_options_t * subscription_options = + &subscription->options; + if (!rmw_fastrtps_shared_cpp::create_datareader( + info->datareader_qos_, + subscription_options, + subscriber, + des_topic, + info->listener_, + &info->data_reader_)) + { + RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); + return RMW_RET_ERROR; + } + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { - RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); - return RMW_RET_ERROR; - } - // lambda to delete datareader - auto cleanup_datareader = rcpputils::make_scope_exit( - [subscriber, info]() - { - subscriber->delete_datareader(info->data_reader_); - }); + subscriber->delete_datareader(info->data_reader_); + }); - ///// - // Update RMW GID - info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( - eprosima_fastrtps_identifier, info->data_reader_->guid()); + ///// + // Update RMW GID + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->data_reader_->guid()); - { - rmw_dds_common::Context* common_context = info->common_context_; - const rmw_node_t* node = info->node_; + { + rmw_dds_common::Context * common_context = info->common_context_; + const rmw_node_t * node = info->node_; - // Update graph - std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.associate_reader( - info->subscription_gid_, common_context->gid, node->name, node->namespace_); - rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( - eprosima_fastrtps_identifier, - common_context->pub, - static_cast(&msg), - nullptr); - if (RMW_RET_OK != rmw_ret) - { - static_cast(common_context->graph_cache.dissociate_writer( - info->subscription_gid_, common_context->gid, node->name, node->namespace_)); - return RMW_RET_ERROR; - } + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + static_cast(common_context->graph_cache.dissociate_writer( + info->subscription_gid_, common_context->gid, node->name, node->namespace_)); + return RMW_RET_ERROR; } - cleanup_datareader.cancel(); - return RMW_RET_OK; + } + cleanup_datareader.cancel(); + return RMW_RET_OK; } rmw_ret_t __rmw_subscription_get_content_filter( - const rmw_subscription_t* subscription, - rcutils_allocator_t* allocator, - rmw_subscription_content_filter_options_t* options - ) + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_subscription_content_filter_options_t * options +) { - auto info = static_cast(subscription->data); - eprosima::fastdds::dds::ContentFilteredTopic* filtered_topic = info->filtered_topic_; + auto info = static_cast(subscription->data); + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = info->filtered_topic_; - if (nullptr == filtered_topic) - { - RMW_SET_ERROR_MSG("this subscriber has not created a ContentFilteredTopic"); - return RMW_RET_ERROR; - } - std::vector expression_parameters; - ReturnCode_t ret = filtered_topic->get_expression_parameters(expression_parameters); - if (ret != ReturnCode_t::RETCODE_OK) - { - RMW_SET_ERROR_MSG( - "failed to get_expression_parameters"); - return RMW_RET_ERROR; - } + if (nullptr == filtered_topic) { + RMW_SET_ERROR_MSG("this subscriber has not created a ContentFilteredTopic"); + return RMW_RET_ERROR; + } + std::vector expression_parameters; + ReturnCode_t ret = filtered_topic->get_expression_parameters(expression_parameters); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG( + "failed to get_expression_parameters"); + return RMW_RET_ERROR; + } - std::vector string_array; - for (size_t i = 0; i < expression_parameters.size(); ++i) - { - string_array.push_back(expression_parameters[i].c_str()); - } + std::vector string_array; + for (size_t i = 0; i < expression_parameters.size(); ++i) { + string_array.push_back(expression_parameters[i].c_str()); + } - return rmw_subscription_content_filter_options_init( - filtered_topic->get_filter_expression().c_str(), - string_array.size(), - string_array.data(), - allocator, - options - ); + return rmw_subscription_content_filter_options_init( + filtered_topic->get_filter_expression().c_str(), + string_array.size(), + string_array.data(), + allocator, + options + ); } rmw_ret_t __rmw_subscription_set_on_new_message_callback( - rmw_subscription_t* rmw_subscription, - rmw_event_callback_t callback, - const void* user_data) + rmw_subscription_t * rmw_subscription, + rmw_event_callback_t callback, + const void * user_data) { - auto custom_subscriber_info = static_cast(rmw_subscription->data); - custom_subscriber_info->listener_->set_on_new_message_callback( - user_data, - callback); - return RMW_RET_OK; + auto custom_subscriber_info = static_cast(rmw_subscription->data); + custom_subscriber_info->listener_->set_on_new_message_callback( + user_data, + callback); + return RMW_RET_OK; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp index 8405c1b6d..89ba122ce 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_take.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_take.cpp @@ -36,533 +36,502 @@ #include "tracetools/tracetools.h" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ using DataSharingKind = eprosima::fastdds::dds::DataSharingKind; void _assign_message_info( - const char* identifier, - rmw_message_info_t* message_info, - const eprosima::fastdds::dds::SampleInfo* sinfo) + const char * identifier, + rmw_message_info_t * message_info, + const eprosima::fastdds::dds::SampleInfo * sinfo) { - message_info->source_timestamp = sinfo->source_timestamp.to_ns(); - message_info->received_timestamp = sinfo->reception_timestamp.to_ns(); - auto fastdds_sn = sinfo->sample_identity.sequence_number(); - message_info->publication_sequence_number = - (static_cast(fastdds_sn.high) << 32) | - static_cast(fastdds_sn.low); - message_info->reception_sequence_number = RMW_MESSAGE_INFO_SEQUENCE_NUMBER_UNSUPPORTED; - rmw_gid_t* sender_gid = &message_info->publisher_gid; - sender_gid->implementation_identifier = identifier; - memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE); - - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( - sinfo->sample_identity.writer_guid(), - sender_gid->data); + message_info->source_timestamp = sinfo->source_timestamp.to_ns(); + message_info->received_timestamp = sinfo->reception_timestamp.to_ns(); + auto fastdds_sn = sinfo->sample_identity.sequence_number(); + message_info->publication_sequence_number = + (static_cast(fastdds_sn.high) << 32) | + static_cast(fastdds_sn.low); + message_info->reception_sequence_number = RMW_MESSAGE_INFO_SEQUENCE_NUMBER_UNSUPPORTED; + rmw_gid_t * sender_gid = &message_info->publisher_gid; + sender_gid->implementation_identifier = identifier; + memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE); + + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + sinfo->sample_identity.writer_guid(), + sender_gid->data); } rmw_ret_t _take( - const char* identifier, - const rmw_subscription_t* subscription, - void* ros_message, - bool* taken, - rmw_message_info_t* message_info, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) { - (void) allocation; - *taken = false; - - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription handle, - subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) - - auto info = static_cast(subscription->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - - eprosima::fastdds::dds::SampleInfo sinfo; - - rmw_fastrtps_shared_cpp::SerializedData data; - - data.is_cdr_buffer = false; - data.data = ros_message; - data.impl = info->type_support_impl_; - - while (0 < info->data_reader_->get_unread_count()) - { - if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) - { - - if (subscription->options.ignore_local_publications) - { - auto sample_writer_guid = - eprosima::fastrtps::rtps::iHandle2GUID(sinfo.publication_handle); - - if (sample_writer_guid.guidPrefix == info->data_reader_->guid().guidPrefix) - { - // This is a local publication. Ignore it - continue; - } - } - - if (sinfo.valid_data) - { - if (message_info) - { - _assign_message_info(identifier, message_info, &sinfo); - } - *taken = true; - break; - } + (void) allocation; + *taken = false; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); + + eprosima::fastdds::dds::SampleInfo sinfo; + + rmw_fastrtps_shared_cpp::SerializedData data; + + data.is_cdr_buffer = false; + data.data = ros_message; + data.impl = info->type_support_impl_; + + while (0 < info->data_reader_->get_unread_count()) { + if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { + if (subscription->options.ignore_local_publications) { + auto sample_writer_guid = + eprosima::fastrtps::rtps::iHandle2GUID(sinfo.publication_handle); + + if (sample_writer_guid.guidPrefix == info->data_reader_->guid().guidPrefix) { + // This is a local publication. Ignore it + continue; } - } + } - TRACEPOINT( - rmw_take, - static_cast(subscription), - static_cast(ros_message), - (message_info ? message_info->source_timestamp : 0LL), - *taken); - return RMW_RET_OK; + if (sinfo.valid_data) { + if (message_info) { + _assign_message_info(identifier, message_info, &sinfo); + } + *taken = true; + break; + } + } + } + + TRACEPOINT( + rmw_take, + static_cast(subscription), + static_cast(ros_message), + (message_info ? message_info->source_timestamp : 0LL), + *taken); + return RMW_RET_OK; } rmw_ret_t _take_sequence( - const char* identifier, - const rmw_subscription_t* subscription, - size_t count, - rmw_message_sequence_t* message_sequence, - rmw_message_info_sequence_t* message_info_sequence, - size_t* taken, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + size_t count, + rmw_message_sequence_t * message_sequence, + rmw_message_info_sequence_t * message_info_sequence, + size_t * taken, + rmw_subscription_allocation_t * allocation) { - *taken = 0; - bool taken_flag = false; - rmw_ret_t ret = RMW_RET_OK; - - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription handle, - subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - - auto info = static_cast(subscription->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - - // Limit the upper bound of reads to the number unread at the beginning. - // This prevents any samples that are added after the beginning of the - // _take_sequence call from being read. - auto unread_count = info->data_reader_->get_unread_count(); - if (unread_count < count) - { - count = unread_count; + *taken = 0; + bool taken_flag = false; + rmw_ret_t ret = RMW_RET_OK; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); + + // Limit the upper bound of reads to the number unread at the beginning. + // This prevents any samples that are added after the beginning of the + // _take_sequence call from being read. + auto unread_count = info->data_reader_->get_unread_count(); + if (unread_count < count) { + count = unread_count; + } + + for (size_t ii = 0; ii < count; ++ii) { + taken_flag = false; + ret = _take( + identifier, subscription, message_sequence->data[*taken], + &taken_flag, &message_info_sequence->data[*taken], allocation); + + if (ret != RMW_RET_OK) { + break; } - for (size_t ii = 0; ii < count; ++ii) - { - taken_flag = false; - ret = _take( - identifier, subscription, message_sequence->data[*taken], - &taken_flag, &message_info_sequence->data[*taken], allocation); - - if (ret != RMW_RET_OK) - { - break; - } - - if (taken_flag) - { - (*taken)++; - } + if (taken_flag) { + (*taken)++; } + } - message_sequence->size = *taken; - message_info_sequence->size = *taken; + message_sequence->size = *taken; + message_info_sequence->size = *taken; - return ret; + return ret; } rmw_ret_t __rmw_take_event( - const char* identifier, - const rmw_event_t* event_handle, - void* event_info, - bool* taken) + const char * identifier, + const rmw_event_t * event_handle, + void * event_info, + bool * taken) { - RMW_CHECK_ARGUMENT_FOR_NULL(event_handle, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(event_info, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(event_handle, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(event_info, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - *taken = false; + *taken = false; - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - event handle, - event_handle->implementation_identifier, - identifier, - return RMW_RET_ERROR); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + event handle, + event_handle->implementation_identifier, + identifier, + return RMW_RET_ERROR); - auto event = static_cast(event_handle->data); - if (event->get_listener()->take_event(event_handle->event_type, event_info)) - { - *taken = true; - return RMW_RET_OK; - } + auto event = static_cast(event_handle->data); + if (event->get_listener()->take_event(event_handle->event_type, event_info)) { + *taken = true; + return RMW_RET_OK; + } - return RMW_RET_ERROR; + return RMW_RET_ERROR; } rmw_ret_t __rmw_take( - const char* identifier, - const rmw_subscription_t* subscription, - void* ros_message, - bool* taken, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_subscription_allocation_t * allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - ros_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + ros_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - return _take(identifier, subscription, ros_message, taken, nullptr, allocation); + return _take(identifier, subscription, ros_message, taken, nullptr, allocation); } rmw_ret_t __rmw_take_sequence( - const char* identifier, - const rmw_subscription_t* subscription, - size_t count, - rmw_message_sequence_t* message_sequence, - rmw_message_info_sequence_t* message_info_sequence, - size_t* taken, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + size_t count, + rmw_message_sequence_t * message_sequence, + rmw_message_info_sequence_t * message_info_sequence, + size_t * taken, + rmw_subscription_allocation_t * allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - message_sequence, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + message_sequence, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - message_info_sequence, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + message_info_sequence, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - if (0u == count) - { - RMW_SET_ERROR_MSG("count cannot be 0"); - return RMW_RET_INVALID_ARGUMENT; - } + if (0u == count) { + RMW_SET_ERROR_MSG("count cannot be 0"); + return RMW_RET_INVALID_ARGUMENT; + } - if (count > message_sequence->capacity) - { - RMW_SET_ERROR_MSG("Insufficient capacity in message_sequence"); - return RMW_RET_INVALID_ARGUMENT; - } + if (count > message_sequence->capacity) { + RMW_SET_ERROR_MSG("Insufficient capacity in message_sequence"); + return RMW_RET_INVALID_ARGUMENT; + } - if (count > message_info_sequence->capacity) - { - RMW_SET_ERROR_MSG("Insufficient capacity in message_info_sequence"); - return RMW_RET_INVALID_ARGUMENT; - } + if (count > message_info_sequence->capacity) { + RMW_SET_ERROR_MSG("Insufficient capacity in message_info_sequence"); + return RMW_RET_INVALID_ARGUMENT; + } - return _take_sequence( - identifier, subscription, count, message_sequence, message_info_sequence, - taken, allocation); + return _take_sequence( + identifier, subscription, count, message_sequence, message_info_sequence, + taken, allocation); } rmw_ret_t __rmw_take_with_info( - const char* identifier, - const rmw_subscription_t* subscription, - void* ros_message, - bool* taken, - rmw_message_info_t* message_info, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - message_info, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + message_info, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - ros_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + ros_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - return _take(identifier, subscription, ros_message, taken, message_info, allocation); + return _take(identifier, subscription, ros_message, taken, message_info, allocation); } rmw_ret_t _take_serialized_message( - const char* identifier, - const rmw_subscription_t* subscription, - rmw_serialized_message_t* serialized_message, - bool* taken, - rmw_message_info_t* message_info, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) { - (void) allocation; - *taken = false; - - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription handle, - subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) - - auto info = static_cast(subscription->data); - RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); - - eprosima::fastcdr::FastBuffer buffer; - eprosima::fastdds::dds::SampleInfo sinfo; - - rmw_fastrtps_shared_cpp::SerializedData data; - data.is_cdr_buffer = true; - data.data = &buffer; - data.impl = nullptr; // not used when is_cdr_buffer is true - - if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) - { - if (sinfo.valid_data) - { - auto buffer_size = static_cast(buffer.getBufferSize()); - if (serialized_message->buffer_capacity < buffer_size) - { - auto ret = rmw_serialized_message_resize(serialized_message, buffer_size); - if (ret != RMW_RET_OK) - { - return ret; // Error message already set - } - } - serialized_message->buffer_length = buffer_size; - memcpy(serialized_message->buffer, buffer.getBuffer(), serialized_message->buffer_length); - - if (message_info) - { - _assign_message_info(identifier, message_info, &sinfo); - } - *taken = true; + (void) allocation; + *taken = false; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); + + eprosima::fastcdr::FastBuffer buffer; + eprosima::fastdds::dds::SampleInfo sinfo; + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = &buffer; + data.impl = nullptr; // not used when is_cdr_buffer is true + + if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { + if (sinfo.valid_data) { + auto buffer_size = static_cast(buffer.getBufferSize()); + if (serialized_message->buffer_capacity < buffer_size) { + auto ret = rmw_serialized_message_resize(serialized_message, buffer_size); + if (ret != RMW_RET_OK) { + return ret; // Error message already set } + } + serialized_message->buffer_length = buffer_size; + memcpy(serialized_message->buffer, buffer.getBuffer(), serialized_message->buffer_length); + + if (message_info) { + _assign_message_info(identifier, message_info, &sinfo); + } + *taken = true; } + } - return RMW_RET_OK; + return RMW_RET_OK; } rmw_ret_t __rmw_take_serialized_message( - const char* identifier, - const rmw_subscription_t* subscription, - rmw_serialized_message_t* serialized_message, - bool* taken, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_subscription_allocation_t * allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - serialized_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + serialized_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - return _take_serialized_message( - identifier, subscription, serialized_message, taken, nullptr, allocation); + return _take_serialized_message( + identifier, subscription, serialized_message, taken, nullptr, allocation); } rmw_ret_t __rmw_take_serialized_message_with_info( - const char* identifier, - const rmw_subscription_t* subscription, - rmw_serialized_message_t* serialized_message, - bool* taken, - rmw_message_info_t* message_info, - rmw_subscription_allocation_t* allocation) + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) { - RMW_CHECK_ARGUMENT_FOR_NULL( - subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - serialized_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + serialized_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - message_info, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL( + message_info, RMW_RET_INVALID_ARGUMENT); - return _take_serialized_message( - identifier, subscription, serialized_message, taken, message_info, allocation); + return _take_serialized_message( + identifier, subscription, serialized_message, taken, message_info, allocation); } // ----------------- Loans related code ------------------------- // struct GenericSequence : public eprosima::fastdds::dds::LoanableCollection { - GenericSequence() = default; - - void resize( - size_type /*new_length*/) override - { - // This kind of collection should only be used with loans - throw std::bad_alloc(); - } - + GenericSequence() = default; + + void resize( + size_type /*new_length*/) override + { + // This kind of collection should only be used with loans + throw std::bad_alloc(); + } }; struct LoanManager { - struct Item - { - GenericSequence data_seq{}; - eprosima::fastdds::dds::SampleInfoSeq info_seq{}; - }; - - explicit LoanManager( - const eprosima::fastrtps::ResourceLimitedContainerConfig& items_cfg) - : items(items_cfg) - { + struct Item + { + GenericSequence data_seq{}; + eprosima::fastdds::dds::SampleInfoSeq info_seq{}; + }; + + explicit LoanManager( + const eprosima::fastrtps::ResourceLimitedContainerConfig & items_cfg) + : items(items_cfg) + { + } + + void add_item( + std::unique_ptr item) + { + std::lock_guard guard(mtx); + items.push_back(std::move(item)); + } + + std::unique_ptr erase_item( + void * loaned_message) + { + std::unique_ptr ret{nullptr}; + + std::lock_guard guard(mtx); + for (auto it = items.begin(); it != items.end(); ++it) { + if (loaned_message == (*it)->data_seq.buffer()[0]) { + ret = std::move(*it); + items.erase(it); + break; + } } - void add_item( - std::unique_ptr item) - { - std::lock_guard guard(mtx); - items.push_back(std::move(item)); - } - - std::unique_ptr erase_item( - void* loaned_message) - { - std::unique_ptr ret{nullptr}; - - std::lock_guard guard(mtx); - for (auto it = items.begin(); it != items.end(); ++it) - { - if (loaned_message == (*it)->data_seq.buffer()[0]) - { - ret = std::move(*it); - items.erase(it); - break; - } - } - - return ret; - } + return ret; + } private: - - std::mutex mtx; - using ItemVector = eprosima::fastrtps::ResourceLimitedVector>; - ItemVector items RCPPUTILS_TSA_GUARDED_BY( - mtx); + std::mutex mtx; + using ItemVector = eprosima::fastrtps::ResourceLimitedVector>; + ItemVector items RCPPUTILS_TSA_GUARDED_BY( + mtx); }; void __init_subscription_for_loans( - rmw_subscription_t* subscription) + rmw_subscription_t * subscription) { - auto info = static_cast(subscription->data); - const auto& qos = info->data_reader_->get_qos(); - bool has_data_sharing = DataSharingKind::OFF != qos.data_sharing().kind(); - subscription->can_loan_messages = has_data_sharing && info->type_support_->is_plain(); - if (subscription->can_loan_messages) - { - const auto& allocation_qos = qos.reader_resource_limits().outstanding_reads_allocation; - info->loan_manager_ = std::make_shared(allocation_qos); - } + auto info = static_cast(subscription->data); + const auto & qos = info->data_reader_->get_qos(); + bool has_data_sharing = DataSharingKind::OFF != qos.data_sharing().kind(); + subscription->can_loan_messages = has_data_sharing && info->type_support_->is_plain(); + if (subscription->can_loan_messages) { + const auto & allocation_qos = qos.reader_resource_limits().outstanding_reads_allocation; + info->loan_manager_ = std::make_shared(allocation_qos); + } } rmw_ret_t __rmw_take_loaned_message_internal( - const char* identifier, - const rmw_subscription_t* subscription, - void** loaned_message, - bool* taken, - rmw_message_info_t* message_info) + const char * identifier, + const rmw_subscription_t * subscription, + void ** loaned_message, + bool * taken, + rmw_message_info_t * message_info) { - RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription, subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (!subscription->can_loan_messages) - { - RMW_SET_ERROR_MSG("Loaning is not supported"); - return RMW_RET_UNSUPPORTED; - } - - RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!subscription->can_loan_messages) { + RMW_SET_ERROR_MSG("Loaning is not supported"); + return RMW_RET_UNSUPPORTED; + } - auto info = static_cast(subscription->data); + RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - auto item = std::make_unique(); + auto info = static_cast(subscription->data); - while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(item->data_seq, item->info_seq, 1)) - { - if (item->info_seq[0].valid_data) - { - if (nullptr != message_info) - { - _assign_message_info(identifier, message_info, &item->info_seq[0]); - } - *loaned_message = item->data_seq.buffer()[0]; - *taken = true; + auto item = std::make_unique(); - info->loan_manager_->add_item(std::move(item)); + while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(item->data_seq, item->info_seq, 1)) { + if (item->info_seq[0].valid_data) { + if (nullptr != message_info) { + _assign_message_info(identifier, message_info, &item->info_seq[0]); + } + *loaned_message = item->data_seq.buffer()[0]; + *taken = true; - return RMW_RET_OK; - } + info->loan_manager_->add_item(std::move(item)); - // Should return loan before taking again - info->data_reader_->return_loan(item->data_seq, item->info_seq); + return RMW_RET_OK; } - // No data available, return loan information. - *taken = false; - return RMW_RET_OK; + // Should return loan before taking again + info->data_reader_->return_loan(item->data_seq, item->info_seq); + } + + // No data available, return loan information. + *taken = false; + return RMW_RET_OK; } rmw_ret_t __rmw_return_loaned_message_from_subscription( - const char* identifier, - const rmw_subscription_t* subscription, - void* loaned_message) + const char * identifier, + const rmw_subscription_t * subscription, + void * loaned_message) { - RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription, subscription->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (!subscription->can_loan_messages) - { - RMW_SET_ERROR_MSG("Loaning is not supported"); - return RMW_RET_UNSUPPORTED; + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!subscription->can_loan_messages) { + RMW_SET_ERROR_MSG("Loaning is not supported"); + return RMW_RET_UNSUPPORTED; + } + RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); + + auto info = static_cast(subscription->data); + std::unique_ptr item; + item = info->loan_manager_->erase_item(loaned_message); + if (item != nullptr) { + if (!info->data_reader_->return_loan(item->data_seq, item->info_seq)) { + RMW_SET_ERROR_MSG("Error returning loan"); + return RMW_RET_ERROR; } - RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); - - auto info = static_cast(subscription->data); - std::unique_ptr item; - item = info->loan_manager_->erase_item(loaned_message); - if (item != nullptr) - { - if (!info->data_reader_->return_loan(item->data_seq, item->info_seq)) - { - RMW_SET_ERROR_MSG("Error returning loan"); - return RMW_RET_ERROR; - } - return RMW_RET_OK; - } + return RMW_RET_OK; + } - RMW_SET_ERROR_MSG("Trying to return message not loaned by this subscription"); - return RMW_RET_ERROR; + RMW_SET_ERROR_MSG("Trying to return message not loaned by this subscription"); + return RMW_RET_ERROR; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp b/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp index 45b5287bc..0f3494ff8 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_trigger_guard_condition.cpp @@ -35,7 +35,8 @@ __rmw_trigger_guard_condition( return RMW_RET_ERROR; } - auto guard_condition = static_cast(guard_condition_handle->data); + auto guard_condition = + static_cast(guard_condition_handle->data); guard_condition->set_trigger_value(true); return RMW_RET_OK; } diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp index db7b33c79..a0095c930 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait.cpp @@ -27,275 +27,255 @@ #include "fastdds/dds/core/condition/GuardCondition.hpp" #include "fastdds/dds/subscriber/DataReader.hpp" -namespace rmw_fastrtps_shared_cpp { +namespace rmw_fastrtps_shared_cpp +{ rmw_ret_t __rmw_wait( - const char* identifier, - rmw_subscriptions_t* subscriptions, - rmw_guard_conditions_t* guard_conditions, - rmw_services_t* services, - rmw_clients_t* clients, - rmw_events_t* events, - rmw_wait_set_t* wait_set, - const rmw_time_t* wait_timeout) + const char * identifier, + rmw_subscriptions_t * subscriptions, + rmw_guard_conditions_t * guard_conditions, + rmw_services_t * services, + rmw_clients_t * clients, + rmw_events_t * events, + rmw_wait_set_t * wait_set, + const rmw_time_t * wait_timeout) { - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INVALID_ARGUMENT); - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - wait set handle, - wait_set->implementation_identifier, identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + RMW_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + wait set handle, + wait_set->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) - // If wait_set_info is ever nullptr, it can only mean one of three things: - // - Wait set is invalid. Caller did not respect preconditions. - // - Implementation is logically broken. Definitely not something we want to treat as a normal - // error. - // - Heap is corrupt. - // In all three cases, it's better if this crashes soon enough. - auto fastdds_wait_set = static_cast(wait_set->data); - bool no_has_wait = false; + // If wait_set_info is ever nullptr, it can only mean one of three things: + // - Wait set is invalid. Caller did not respect preconditions. + // - Implementation is logically broken. Definitely not something we want to treat as a normal + // error. + // - Heap is corrupt. + // In all three cases, it's better if this crashes soon enough. + auto fastdds_wait_set = static_cast(wait_set->data); + bool no_has_wait = false; - if (subscriptions) - { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) - { - void* data = subscriptions->subscribers[i]; - auto custom_subscriber_info = static_cast(data); - no_has_wait |= (0 < custom_subscriber_info->data_reader_->get_unread_count()); - fastdds_wait_set->attach_condition(custom_subscriber_info->data_reader_->get_statuscondition()); - } + if (subscriptions) { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + void * data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + no_has_wait |= (0 < custom_subscriber_info->data_reader_->get_unread_count()); + fastdds_wait_set->attach_condition( + custom_subscriber_info->data_reader_->get_statuscondition()); } + } - if (clients) - { - for (size_t i = 0; i < clients->client_count; ++i) - { - void* data = clients->clients[i]; - auto custom_client_info = static_cast(data); - no_has_wait |= (0 < custom_client_info->response_reader_->get_unread_count()); - fastdds_wait_set->attach_condition(custom_client_info->response_reader_->get_statuscondition()); - } + if (clients) { + for (size_t i = 0; i < clients->client_count; ++i) { + void * data = clients->clients[i]; + auto custom_client_info = static_cast(data); + no_has_wait |= (0 < custom_client_info->response_reader_->get_unread_count()); + fastdds_wait_set->attach_condition( + custom_client_info->response_reader_->get_statuscondition()); } + } - if (services) - { - for (size_t i = 0; i < services->service_count; ++i) - { - void* data = services->services[i]; - auto custom_service_info = static_cast(data); - no_has_wait |= (0 < custom_service_info->request_reader_->get_unread_count()); - fastdds_wait_set->attach_condition(custom_service_info->request_reader_->get_statuscondition()); - } + if (services) { + for (size_t i = 0; i < services->service_count; ++i) { + void * data = services->services[i]; + auto custom_service_info = static_cast(data); + no_has_wait |= (0 < custom_service_info->request_reader_->get_unread_count()); + fastdds_wait_set->attach_condition( + custom_service_info->request_reader_->get_statuscondition()); } + } - if (events) - { - for (size_t i = 0; i < events->event_count; ++i) - { - auto event = static_cast(events->events[i]); - auto custom_event_info = static_cast(event->data); - fastdds_wait_set->attach_condition(custom_event_info->get_listener()->get_statuscondition()); - fastdds_wait_set->attach_condition(custom_event_info->get_listener()->event_guard[event->event_type]); - } + if (events) { + for (size_t i = 0; i < events->event_count; ++i) { + auto event = static_cast(events->events[i]); + auto custom_event_info = static_cast(event->data); + fastdds_wait_set->attach_condition( + custom_event_info->get_listener()->get_statuscondition()); + fastdds_wait_set->attach_condition( + custom_event_info->get_listener()->event_guard[event-> + event_type]); } + } - if (guard_conditions) - { - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) - { - void* data = guard_conditions->guard_conditions[i]; - auto guard_condition = static_cast(data); - fastdds_wait_set->attach_condition(*guard_condition); - } + if (guard_conditions) { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + void * data = guard_conditions->guard_conditions[i]; + auto guard_condition = static_cast(data); + fastdds_wait_set->attach_condition(*guard_condition); } + } - eprosima::fastdds::dds::ConditionSeq triggered_coditions; - Duration_t timeout{0, 0}; - if (!no_has_wait) - { - timeout = (wait_timeout) ? - Duration_t{static_cast(wait_timeout->sec), - static_cast(wait_timeout->nsec)} : eprosima::fastrtps::c_TimeInfinite; - } + eprosima::fastdds::dds::ConditionSeq triggered_coditions; + Duration_t timeout{0, 0}; + if (!no_has_wait) { + timeout = (wait_timeout) ? + Duration_t{static_cast(wait_timeout->sec), + static_cast(wait_timeout->nsec)} : eprosima::fastrtps::c_TimeInfinite; + } - ReturnCode_t ret_code = fastdds_wait_set->wait(triggered_coditions, - timeout - ); + ReturnCode_t ret_code = fastdds_wait_set->wait( + triggered_coditions, + timeout + ); - if (subscriptions) - { - for (size_t i = 0; i < subscriptions->subscriber_count; ++i) - { - void* data = subscriptions->subscribers[i]; - auto custom_subscriber_info = static_cast(data); - eprosima::fastdds::dds::StatusCondition& status_condition = - custom_subscriber_info->data_reader_->get_statuscondition(); - fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition* condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) - { - subscriptions->subscribers[i] = 0; - } - } - else if (0 == custom_subscriber_info->data_reader_->get_unread_count()) - { - subscriptions->subscribers[i] = 0; - } + if (subscriptions) { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + void * data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + eprosima::fastdds::dds::StatusCondition & status_condition = + custom_subscriber_info->data_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition * condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if( + triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition * c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) { + subscriptions->subscribers[i] = 0; } + } else if (0 == custom_subscriber_info->data_reader_->get_unread_count()) { + subscriptions->subscribers[i] = 0; + } } + } - if (clients) - { - for (size_t i = 0; i < clients->client_count; ++i) - { - void* data = clients->clients[i]; - auto custom_client_info = static_cast(data); - eprosima::fastdds::dds::StatusCondition& status_condition = - custom_client_info->response_reader_->get_statuscondition(); - fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition* condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) - { - clients->clients[i] = 0; - } - } - else if (0 == custom_client_info->response_reader_->get_unread_count()) - { - clients->clients[i] = 0; - } + if (clients) { + for (size_t i = 0; i < clients->client_count; ++i) { + void * data = clients->clients[i]; + auto custom_client_info = static_cast(data); + eprosima::fastdds::dds::StatusCondition & status_condition = + custom_client_info->response_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition * condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if( + triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition * c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) { + clients->clients[i] = 0; } + } else if (0 == custom_client_info->response_reader_->get_unread_count()) { + clients->clients[i] = 0; + } } + } - if (services) - { - for (size_t i = 0; i < services->service_count; ++i) - { - void* data = services->services[i]; - auto custom_service_info = static_cast(data); - eprosima::fastdds::dds::StatusCondition& status_condition = - custom_service_info->request_reader_->get_statuscondition(); - fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition* condition = &status_condition; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) - { - services->services[i] = 0; - } - } - else if (0 == custom_service_info->request_reader_->get_unread_count()) - { - services->services[i] = 0; - } + if (services) { + for (size_t i = 0; i < services->service_count; ++i) { + void * data = services->services[i]; + auto custom_service_info = static_cast(data); + eprosima::fastdds::dds::StatusCondition & status_condition = + custom_service_info->request_reader_->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition * condition = &status_condition; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if( + triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition * c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (!changed_statuses.is_active(eprosima::fastdds::dds::StatusMask::data_available())) { + services->services[i] = 0; } + } else if (0 == custom_service_info->request_reader_->get_unread_count()) { + services->services[i] = 0; + } } + } - if (events) - { - for (size_t i = 0; i < events->event_count; ++i) + if (events) { + for (size_t i = 0; i < events->event_count; ++i) { + auto event = static_cast(events->events[i]); + auto custom_event_info = static_cast(event->data); + fastdds_wait_set->detach_condition(custom_event_info->get_listener()->get_statuscondition()); + eprosima::fastdds::dds::StatusCondition & status_condition = + custom_event_info->get_listener()->get_statuscondition(); + fastdds_wait_set->detach_condition(status_condition); + eprosima::fastdds::dds::Condition * condition = &status_condition; + eprosima::fastdds::dds::GuardCondition * guard_condition = + &custom_event_info->get_listener()->event_guard[event->event_type]; + bool active = false; + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if( + triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition * c) + { + return c == condition; + })) + { + eprosima::fastdds::dds::Entity * entity = status_condition.get_entity(); + eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); + if (changed_statuses.is_active( + rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( + event->event_type))) { - auto event = static_cast(events->events[i]); - auto custom_event_info = static_cast(event->data); - fastdds_wait_set->detach_condition(custom_event_info->get_listener()->get_statuscondition()); - eprosima::fastdds::dds::StatusCondition& status_condition = - custom_event_info->get_listener()->get_statuscondition(); - fastdds_wait_set->detach_condition(status_condition); - eprosima::fastdds::dds::Condition* condition = &status_condition; - eprosima::fastdds::dds::GuardCondition* guard_condition = - &custom_event_info->get_listener()->event_guard[event->event_type]; - bool active = false; - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - eprosima::fastdds::dds::Entity* entity = status_condition.get_entity(); - eprosima::fastdds::dds::StatusMask changed_statuses = entity->get_status_changes(); - if (changed_statuses.is_active(rmw_fastrtps_shared_cpp::internal::rmw_event_to_dds_statusmask( - event->event_type))) - { - active = true; - } - } - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [guard_condition](const eprosima::fastdds::dds::Condition* c) - { - return c == guard_condition; - })) - { - if (guard_condition->get_trigger_value()) - { - active = true; - guard_condition->set_trigger_value(false); - } - } + active = true; + } + } + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if( + triggered_coditions.begin(), triggered_coditions.end(), + [guard_condition](const eprosima::fastdds::dds::Condition * c) + { + return c == guard_condition; + })) + { + if (guard_condition->get_trigger_value()) { + active = true; + guard_condition->set_trigger_value(false); + } + } - if (!active) - { - events->events[i] = 0; - } - } + if (!active) { + events->events[i] = 0; + } } + } - if (guard_conditions) - { - for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) - { - void* data = guard_conditions->guard_conditions[i]; - auto condition = static_cast(data); - fastdds_wait_set->detach_condition(*condition); - if (ReturnCode_t::RETCODE_OK == ret_code && - triggered_coditions.end() != std::find_if(triggered_coditions.begin(), triggered_coditions.end(), - [condition](const eprosima::fastdds::dds::Condition* c) - { - return c == condition; - })) - { - if (!condition->get_trigger_value()) - { - guard_conditions->guard_conditions[i] = 0; - } - } - else - { - guard_conditions->guard_conditions[i] = 0; - } - condition->set_trigger_value(false); + if (guard_conditions) { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + void * data = guard_conditions->guard_conditions[i]; + auto condition = static_cast(data); + fastdds_wait_set->detach_condition(*condition); + if (ReturnCode_t::RETCODE_OK == ret_code && + triggered_coditions.end() != std::find_if( + triggered_coditions.begin(), triggered_coditions.end(), + [condition](const eprosima::fastdds::dds::Condition * c) + { + return c == condition; + })) + { + if (!condition->get_trigger_value()) { + guard_conditions->guard_conditions[i] = 0; } + } else { + guard_conditions->guard_conditions[i] = 0; + } + condition->set_trigger_value(false); } + } - return ((no_has_wait || ReturnCode_t::RETCODE_OK == ret_code) ? RMW_RET_OK : RMW_RET_TIMEOUT); + return (no_has_wait || ReturnCode_t::RETCODE_OK == ret_code) ? RMW_RET_OK : RMW_RET_TIMEOUT; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp b/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp index 8c127418a..b706a5006 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_wait_set.cpp @@ -94,7 +94,8 @@ __rmw_destroy_wait_set(const char * identifier, rmw_wait_set_t * wait_set) if (wait_set->data) { if (fastdds_wait_set) { RMW_TRY_DESTRUCTOR( - fastdds_wait_set->eprosima::fastdds::dds::WaitSet::~WaitSet(), fastdds_wait_set, result = RMW_RET_ERROR) + fastdds_wait_set->eprosima::fastdds::dds::WaitSet::~WaitSet(), fastdds_wait_set, + result = RMW_RET_ERROR) } rmw_free(wait_set->data); } diff --git a/rmw_fastrtps_shared_cpp/src/types/event_types.hpp b/rmw_fastrtps_shared_cpp/src/types/event_types.hpp index 0cc3225d6..47e10f905 100644 --- a/rmw_fastrtps_shared_cpp/src/types/event_types.hpp +++ b/rmw_fastrtps_shared_cpp/src/types/event_types.hpp @@ -27,7 +27,7 @@ namespace internal bool is_event_supported(rmw_event_type_t event_type); eprosima::fastdds::dds::StatusMask rmw_event_to_dds_statusmask( - const rmw_event_type_t event_type); + const rmw_event_type_t event_type); } // namespace internal } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/utils.cpp b/rmw_fastrtps_shared_cpp/src/utils.cpp index f9111588e..37662cf01 100644 --- a/rmw_fastrtps_shared_cpp/src/utils.cpp +++ b/rmw_fastrtps_shared_cpp/src/utils.cpp @@ -205,7 +205,7 @@ create_datareader( des_topic, datareader_qos, listener, - eprosima::fastdds::dds::StatusMask::subscription_matched()); + eprosima::fastdds::dds::StatusMask::subscription_matched()); } return true; }