diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 26d7e69362..561beb5d3e 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -85,6 +85,7 @@ set(${PROJECT_NAME}_SRCS src/rclcpp/publisher_base.cpp src/rclcpp/qos.cpp src/rclcpp/qos_event.cpp + src/rclcpp/qos_overriding_options.cpp src/rclcpp/serialization.cpp src/rclcpp/serialized_message.cpp src/rclcpp/service.cpp diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index 811c18b69f..2205a92a54 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" @@ -24,24 +25,27 @@ #include "rclcpp/publisher_factory.hpp" #include "rclcpp/publisher_options.hpp" #include "rclcpp/qos.hpp" +#include "rclcpp/qos_overriding_options.hpp" +#include "rclcpp/detail/qos_parameters.hpp" + #include "rmw/qos_profiles.h" namespace rclcpp { +namespace detail +{ /// Create and return a publisher of the given MessageT type. -/** - * The NodeT type only needs to have a method called get_node_topics_interface() - * which returns a shared_ptr to a NodeTopicsInterface. - */ template< typename MessageT, typename AllocatorT = std::allocator, typename PublisherT = rclcpp::Publisher, - typename NodeT> + typename NodeParametersT, + typename NodeTopicsT> std::shared_ptr create_publisher( - NodeT & node, + NodeParametersT & node_parameters, + NodeTopicsT & node_topics, const std::string & topic_name, const rclcpp::QoS & qos, const rclcpp::PublisherOptionsWithAllocator & options = ( @@ -49,22 +53,72 @@ create_publisher( ) ) { - // Extract the NodeTopicsInterface from the NodeT. - using rclcpp::node_interfaces::get_node_topics_interface; - auto node_topics = get_node_topics_interface(node); + auto node_topics_interface = rclcpp::node_interfaces::get_node_topics_interface(node_topics); + const rclcpp::QoS & actual_qos = options.qos_overriding_options.get_policy_kinds().size() ? + rclcpp::detail::declare_qos_parameters( + options.qos_overriding_options, node_parameters, + node_topics_interface->resolve_topic_name(topic_name), + qos, rclcpp::detail::PublisherQosParametersTraits{}) : + qos; // Create the publisher. - auto pub = node_topics->create_publisher( + auto pub = node_topics_interface->create_publisher( topic_name, rclcpp::create_publisher_factory(options), - qos + actual_qos ); // Add the publisher to the node topics interface. - node_topics->add_publisher(pub, options.callback_group); + node_topics_interface->add_publisher(pub, options.callback_group); return std::dynamic_pointer_cast(pub); } +} // namespace detail + + +/// Create and return a publisher of the given MessageT type. +/** + * The NodeT type only needs to have a method called get_node_topics_interface() + * which returns a shared_ptr to a NodeTopicsInterface. + */ +template< + typename MessageT, + typename AllocatorT = std::allocator, + typename PublisherT = rclcpp::Publisher, + typename NodeT> +std::shared_ptr +create_publisher( + NodeT & node, + const std::string & topic_name, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) +) +{ + return detail::create_publisher( + node, node, topic_name, qos, options); +} + +/// Create and return a publisher of the given MessageT type. +template< + typename MessageT, + typename AllocatorT = std::allocator, + typename PublisherT = rclcpp::Publisher> +std::shared_ptr +create_publisher( + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics, + const std::string & topic_name, + const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = ( + rclcpp::PublisherOptionsWithAllocator() + ) +) +{ + return detail::create_publisher( + node_parameters, node_topics, topic_name, qos, options); +} } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 9c22d171d1..4121fc4d7e 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -41,44 +41,21 @@ namespace rclcpp { -/// Create and return a subscription of the given MessageT type. -/** - * The NodeT type only needs to have a method called get_node_topics_interface() - * which returns a shared_ptr to a NodeTopicsInterface, or be a - * NodeTopicsInterface pointer itself. - * - * \tparam MessageT - * \tparam CallbackT - * \tparam AllocatorT - * \tparam CallbackMessageT - * \tparam SubscriptionT - * \tparam MessageMemoryStrategyT - * \tparam NodeT - * \param node - * \param topic_name - * \param qos - * \param callback - * \param options - * \param msg_mem_strat - * \return the created subscription - * \throws std::invalid_argument if topic statistics is enabled and the publish period is - * less than or equal to zero. - */ +namespace detail +{ template< typename MessageT, typename CallbackT, - typename AllocatorT = std::allocator, - typename CallbackMessageT = - typename rclcpp::subscription_traits::has_message_type::type, - typename SubscriptionT = rclcpp::Subscription, - typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< - CallbackMessageT, - AllocatorT - >, - typename NodeT> + typename AllocatorT, + typename CallbackMessageT, + typename SubscriptionT, + typename MessageMemoryStrategyT, + typename NodeParametersT, + typename NodeTopicsT> typename std::shared_ptr create_subscription( - NodeT && node, + NodeParametersT & node_parameters, + NodeTopicsT & node_topics, const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback, @@ -91,14 +68,14 @@ create_subscription( ) { using rclcpp::node_interfaces::get_node_topics_interface; - auto node_topics = get_node_topics_interface(std::forward(node)); + auto node_topics_interface = get_node_topics_interface(node_topics); std::shared_ptr> subscription_topic_stats = nullptr; if (rclcpp::detail::resolve_enable_topic_statistics( options, - *node_topics->get_node_base_interface())) + *node_topics_interface->get_node_base_interface())) { if (options.topic_stats_options.publish_period <= std::chrono::milliseconds(0)) { throw std::invalid_argument( @@ -107,15 +84,16 @@ create_subscription( " ms"); } - std::shared_ptr> publisher = - create_publisher( - node, + std::shared_ptr> + publisher = rclcpp::detail::create_publisher( + node_parameters, + node_topics_interface, options.topic_stats_options.publish_topic, qos); subscription_topic_stats = std::make_shared< rclcpp::topic_statistics::SubscriptionTopicStatistics - >(node_topics->get_node_base_interface()->get_name(), publisher); + >(node_topics_interface->get_node_base_interface()->get_name(), publisher); std::weak_ptr< rclcpp::topic_statistics::SubscriptionTopicStatistics @@ -127,14 +105,14 @@ create_subscription( } }; - auto node_timer_interface = node_topics->get_node_timers_interface(); + auto node_timer_interface = node_topics_interface->get_node_timers_interface(); auto timer = create_wall_timer( std::chrono::duration_cast( options.topic_stats_options.publish_period), sub_call_back, options.callback_group, - node_topics->get_node_base_interface(), + node_topics_interface->get_node_base_interface(), node_timer_interface ); @@ -148,11 +126,109 @@ create_subscription( subscription_topic_stats ); - auto sub = node_topics->create_subscription(topic_name, factory, qos); - node_topics->add_subscription(sub, options.callback_group); + const rclcpp::QoS & actual_qos = options.qos_overriding_options.get_policy_kinds().size() ? + rclcpp::detail::declare_qos_parameters( + options.qos_overriding_options, node_parameters, + node_topics_interface->resolve_topic_name(topic_name), + qos, rclcpp::detail::SubscriptionQosParametersTraits{}) : + qos; + + auto sub = node_topics_interface->create_subscription(topic_name, factory, actual_qos); + node_topics_interface->add_subscription(sub, options.callback_group); return std::dynamic_pointer_cast(sub); } +} // namespace detail + +/// Create and return a subscription of the given MessageT type. +/** + * The NodeT type only needs to have a method called get_node_topics_interface() + * which returns a shared_ptr to a NodeTopicsInterface, or be a + * NodeTopicsInterface pointer itself. + * + * \tparam MessageT + * \tparam CallbackT + * \tparam AllocatorT + * \tparam CallbackMessageT + * \tparam SubscriptionT + * \tparam MessageMemoryStrategyT + * \tparam NodeT + * \param node + * \param topic_name + * \param qos + * \param callback + * \param options + * \param msg_mem_strat + * \return the created subscription + * \throws std::invalid_argument if topic statistics is enabled and the publish period is + * less than or equal to zero. + */ +template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename CallbackMessageT = + typename rclcpp::subscription_traits::has_message_type::type, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, + AllocatorT + >, + typename NodeT> +typename std::shared_ptr +create_subscription( + NodeT & node, + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( + MessageMemoryStrategyT::create_default() + ) +) +{ + return rclcpp::detail::create_subscription< + MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT, MessageMemoryStrategyT>( + node, node, topic_name, qos, std::forward(callback), options, msg_mem_strat); +} + +/// Create and return a subscription of the given MessageT type. +/** + * See \ref create_subscription(). + */ +template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename CallbackMessageT = + typename rclcpp::subscription_traits::has_message_type::type, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< + CallbackMessageT, + AllocatorT + >> +typename std::shared_ptr +create_subscription( + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics, + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = ( + rclcpp::SubscriptionOptionsWithAllocator() + ), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( + MessageMemoryStrategyT::create_default() + ) +) +{ + return rclcpp::detail::create_subscription< + MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT, MessageMemoryStrategyT>( + node_parameters, node_topics, topic_name, qos, + std::forward(callback), options, msg_mem_strat); +} } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp new file mode 100644 index 0000000000..cd8d9ffef7 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -0,0 +1,326 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__QOS_PARAMETERS_HPP_ +#define RCLCPP__DETAIL__QOS_PARAMETERS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rcpputils/pointer_traits.hpp" +#include "rmw/qos_string_conversions.h" + +#include "rclcpp/duration.hpp" +#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/qos_overriding_options.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// \internal Trait used to specialize `declare_qos_parameters()` for publishers. +struct PublisherQosParametersTraits +{ + static constexpr const char * entity_type() {return "publisher";} + static constexpr auto allowed_policies() + { + return std::array<::rclcpp::QosPolicyKind, 9> { + QosPolicyKind::AvoidRosNamespaceConventions, + QosPolicyKind::Deadline, + QosPolicyKind::Durability, + QosPolicyKind::History, + QosPolicyKind::Depth, + QosPolicyKind::Lifespan, + QosPolicyKind::Liveliness, + QosPolicyKind::LivelinessLeaseDuration, + QosPolicyKind::Reliability, + }; + } +}; + +/// \internal Trait used to specialize `declare_qos_parameters()` for subscriptions. +struct SubscriptionQosParametersTraits +{ + static constexpr const char * entity_type() {return "subscription";} + static constexpr auto allowed_policies() + { + return std::array<::rclcpp::QosPolicyKind, 8> { + QosPolicyKind::AvoidRosNamespaceConventions, + QosPolicyKind::Deadline, + QosPolicyKind::Durability, + QosPolicyKind::History, + QosPolicyKind::Depth, + QosPolicyKind::Liveliness, + QosPolicyKind::LivelinessLeaseDuration, + QosPolicyKind::Reliability, + }; + } +}; + +/// \internal Returns the given `policy` of the profile `qos` converted to a parameter value. +inline +::rclcpp::ParameterValue +get_default_qos_param_value(rclcpp::QosPolicyKind policy, const rclcpp::QoS & qos); + +/// \internal Modify the given `policy` in `qos` to be `value`. +inline +void +apply_qos_override( + rclcpp::QosPolicyKind policy, rclcpp::ParameterValue value, rclcpp::QoS & qos); + +inline +rclcpp::ParameterValue +declare_parameter_or_get( + rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, + const std::string & param_name, + rclcpp::ParameterValue param_value, + rcl_interfaces::msg::ParameterDescriptor descriptor) +{ + try { + return parameters_interface.declare_parameter( + param_name, param_value, descriptor); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { + return parameters_interface.get_parameter(param_name).get_parameter_value(); + } +} + +/// \internal Declare QoS parameters for the given entity. +/** + * \tparam NodeT Node pointer or reference type. + * \tparam EntityQosParametersTraits A class with two static methods: `entity_type()` and + * `allowed_policies()`. See `PublisherQosParametersTraits` and `SubscriptionQosParametersTraits`. + * \param options User provided options that indicate if QoS parameter overrides should be + * declared or not, which policy can have overrides, and optionally a callback to validate the profile. + * \param node Parameters will be declared using this node. + * \param topic_name Name of the topic of the entity. + * \param default_qos User provided qos. It will be used as a default for the parameters declared. + * \return qos profile based on the user provided parameter overrides. + */ +template +std::enable_if_t< + rclcpp::node_interfaces::has_node_parameters_interface< + decltype(std::declval::type>())>::value || + std::is_same, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value, + rclcpp::QoS> +declare_qos_parameters( + const ::rclcpp::QosOverridingOptions & options, + NodeT & node, + const std::string & topic_name, + const ::rclcpp::QoS & default_qos, + EntityQosParametersTraits) +{ + auto & parameters_interface = *rclcpp::node_interfaces::get_node_parameters_interface(node); + std::string param_prefix; + const auto & id = options.get_id(); + { + std::ostringstream oss{"qos_overrides.", std::ios::ate}; + oss << topic_name << "." << EntityQosParametersTraits::entity_type(); + if (!id.empty()) { + oss << "_" << id; + } + oss << "."; + param_prefix = oss.str(); + } + std::string param_description_suffix; + { + std::ostringstream oss{"} for ", std::ios::ate}; + oss << EntityQosParametersTraits::entity_type() << " {" << topic_name << "}"; + if (!id.empty()) { + oss << " with id {" << id << "}"; + } + param_description_suffix = oss.str(); + } + rclcpp::QoS qos = default_qos; + for (auto policy : EntityQosParametersTraits::allowed_policies()) { + if ( + std::count(options.get_policy_kinds().begin(), options.get_policy_kinds().end(), policy)) + { + std::ostringstream param_name{param_prefix, std::ios::ate}; + param_name << qos_policy_kind_to_cstr(policy); + std::ostringstream param_desciption{"qos policy {", std::ios::ate}; + param_desciption << qos_policy_kind_to_cstr(policy) << param_description_suffix; + rcl_interfaces::msg::ParameterDescriptor descriptor{}; + descriptor.description = param_desciption.str(); + descriptor.read_only = true; + auto value = declare_parameter_or_get( + parameters_interface, param_name.str(), + get_default_qos_param_value(policy, qos), descriptor); + ::rclcpp::detail::apply_qos_override(policy, value, qos); + } + } + const auto & validation_callback = options.get_validation_callback(); + if (validation_callback) { + auto result = validation_callback(qos); + if (!result.successful) { + throw rclcpp::exceptions::InvalidQosOverridesException{ + "validation callback failed: " + result.reason}; + } + } + return qos; +} + +// TODO(ivanpauno): This overload cannot declare the QoS parameters, as a node parameters interface +// was not provided. +template +std::enable_if_t< + !(rclcpp::node_interfaces::has_node_parameters_interface< + decltype(std::declval::type>())>::value || + std::is_same, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value), + rclcpp::QoS> +declare_qos_parameters( + const ::rclcpp::QosOverridingOptions & options, + NodeT &, + const std::string &, + const ::rclcpp::QoS & default_qos, + EntityQosParametersTraits) +{ + if (options.get_policy_kinds().size()) { + std::runtime_error exc{ + "passed non-default qos overriding options without providing a parameters interface"}; + throw exc; + } + return default_qos; +} + +/// \internal Helper function to get a rmw qos policy value from a string. +#define RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( \ + kind_lower, kind_upper, parameter_value, rclcpp_qos) \ + do { \ + auto policy_string = (parameter_value).get(); \ + auto policy_value = rmw_qos_ ## kind_lower ## _policy_from_str(policy_string.c_str()); \ + if (RMW_QOS_POLICY_ ## kind_upper ## _UNKNOWN == policy_value) { \ + throw std::invalid_argument{"unknown QoS policy " #kind_lower " value: " + policy_string}; \ + } \ + ((rclcpp_qos).kind_lower)(policy_value); \ + } while (0) + +inline +void +apply_qos_override( + rclcpp::QosPolicyKind policy, rclcpp::ParameterValue value, rclcpp::QoS & qos) +{ + switch (policy) { + case QosPolicyKind::AvoidRosNamespaceConventions: + qos.avoid_ros_namespace_conventions(value.get()); + break; + case QosPolicyKind::Deadline: + qos.deadline(::rclcpp::Duration::from_nanoseconds(value.get())); + break; + case QosPolicyKind::Durability: + RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( + durability, DURABILITY, value, qos); + break; + case QosPolicyKind::History: + RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( + history, HISTORY, value, qos); + break; + case QosPolicyKind::Depth: + qos.get_rmw_qos_profile().depth = static_cast(value.get()); + break; + case QosPolicyKind::Lifespan: + qos.lifespan(::rclcpp::Duration::from_nanoseconds(value.get())); + break; + case QosPolicyKind::Liveliness: + RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( + liveliness, LIVELINESS, value, qos); + break; + case QosPolicyKind::LivelinessLeaseDuration: + qos.liveliness_lease_duration(::rclcpp::Duration::from_nanoseconds(value.get())); + break; + case QosPolicyKind::Reliability: + RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( + reliability, RELIABILITY, value, qos); + break; + default: + throw std::invalid_argument{"unknown QosPolicyKind"}; + } +} + +/// Convert `rmw_time_t` to `int64_t` that can be used as a parameter value. +inline +int64_t +rmw_duration_to_int64_t(rmw_time_t rmw_duration) +{ + return ::rclcpp::Duration( + static_cast(rmw_duration.sec), + static_cast(rmw_duration.nsec) + ).nanoseconds(); +} + +/// \internal Throw an exception if `policy_value_stringified` is NULL. +inline +const char * +check_if_stringified_policy_is_null(const char * policy_value_stringified, QosPolicyKind kind) +{ + if (!policy_value_stringified) { + std::ostringstream oss{"unknown value for policy kind {", std::ios::ate}; + oss << kind << "}"; + throw std::invalid_argument{oss.str()}; + } + return policy_value_stringified; +} + +inline +::rclcpp::ParameterValue +get_default_qos_param_value(rclcpp::QosPolicyKind kind, const rclcpp::QoS & qos) +{ + using ParameterValue = ::rclcpp::ParameterValue; + const auto & rmw_qos = qos.get_rmw_qos_profile(); + switch (kind) { + case QosPolicyKind::AvoidRosNamespaceConventions: + return ParameterValue(rmw_qos.avoid_ros_namespace_conventions); + case QosPolicyKind::Deadline: + return ParameterValue(rmw_duration_to_int64_t(rmw_qos.deadline)); + case QosPolicyKind::Durability: + return ParameterValue( + check_if_stringified_policy_is_null( + rmw_qos_durability_policy_to_str(rmw_qos.durability), kind)); + case QosPolicyKind::History: + return ParameterValue( + check_if_stringified_policy_is_null( + rmw_qos_history_policy_to_str(rmw_qos.history), kind)); + case QosPolicyKind::Depth: + return ParameterValue(static_cast(rmw_qos.depth)); + case QosPolicyKind::Lifespan: + return ParameterValue(rmw_duration_to_int64_t(rmw_qos.lifespan)); + case QosPolicyKind::Liveliness: + return ParameterValue( + check_if_stringified_policy_is_null( + rmw_qos_liveliness_policy_to_str(rmw_qos.liveliness), kind)); + case QosPolicyKind::LivelinessLeaseDuration: + return ParameterValue(rmw_duration_to_int64_t(rmw_qos.liveliness_lease_duration)); + case QosPolicyKind::Reliability: + return ParameterValue( + check_if_stringified_policy_is_null( + rmw_qos_reliability_policy_to_str(rmw_qos.reliability), kind)); + default: + throw std::invalid_argument{"unknown QoS policy kind"}; + } +} + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__QOS_PARAMETERS_HPP_ diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index 233b4b67ba..630e2d846f 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -282,6 +282,13 @@ class ParameterModifiedInCallbackException : public std::runtime_error using std::runtime_error::runtime_error; }; +/// Thrown if the QoS overrides provided aren't valid. +class InvalidQosOverridesException : public std::runtime_error +{ + // Inherit constructors from runtime_error. + using std::runtime_error::runtime_error; +}; + } // namespace exceptions } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/publisher_options.hpp b/rclcpp/include/rclcpp/publisher_options.hpp index 9547b349dd..ff038d15f4 100644 --- a/rclcpp/include/rclcpp/publisher_options.hpp +++ b/rclcpp/include/rclcpp/publisher_options.hpp @@ -26,6 +26,7 @@ #include "rclcpp/intra_process_setting.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" +#include "rclcpp/qos_overriding_options.hpp" namespace rclcpp { @@ -50,6 +51,8 @@ struct PublisherOptionsBase /// Optional RMW implementation specific payload to be used during creation of the publisher. std::shared_ptr rmw_implementation_payload = nullptr; + + QosOverridingOptions qos_overriding_options; }; /// Structure containing optional configuration for Publishers. diff --git a/rclcpp/include/rclcpp/qos_overriding_options.hpp b/rclcpp/include/rclcpp/qos_overriding_options.hpp new file mode 100644 index 0000000000..c20248de6e --- /dev/null +++ b/rclcpp/include/rclcpp/qos_overriding_options.hpp @@ -0,0 +1,154 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__QOS_OVERRIDING_OPTIONS_HPP_ +#define RCLCPP__QOS_OVERRIDING_OPTIONS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/qos.hpp" +#include "rclcpp/visibility_control.hpp" + +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "rmw/qos_policy_kind.h" + +namespace rclcpp +{ + +enum class RCLCPP_PUBLIC_TYPE QosPolicyKind +{ + AvoidRosNamespaceConventions = RMW_QOS_POLICY_AVOID_ROS_NAMESPACE_CONVENTIONS, + Deadline = RMW_QOS_POLICY_DEADLINE, + Depth = RMW_QOS_POLICY_DEPTH, + Durability = RMW_QOS_POLICY_DURABILITY, + History = RMW_QOS_POLICY_HISTORY, + Lifespan = RMW_QOS_POLICY_LIFESPAN, + Liveliness = RMW_QOS_POLICY_LIVELINESS, + LivelinessLeaseDuration = RMW_QOS_POLICY_LIVELINESS_LEASE_DURATION, + Reliability = RMW_QOS_POLICY_RELIABILITY, + Invalid = RMW_QOS_POLICY_INVALID, +}; + +RCLCPP_PUBLIC +const char * +qos_policy_kind_to_cstr(const QosPolicyKind & qpk); + +RCLCPP_PUBLIC +std::ostream & +operator<<(std::ostream & os, const QosPolicyKind & qpk); + +using QosCallbackResult = rcl_interfaces::msg::SetParametersResult; +using QosCallback = std::function; + +namespace detail +{ +// forward declare +template +class QosParameters; +} + +/// Options that are passed in subscription/publisher constructor to specify QoSConfigurability. +/** + * This options struct allows configuring: + * - Which policy kinds will have declared parameters. + * - An optional callback, that will be called to validate the final qos profile. + * - An optional id. In the case that different qos are desired for two publishers/subscriptions in + * the same topic, this id will allow disambiguating them. + * + * Example parameter file: + * + * ```yaml + * my_node_name: + * ros__parameters: + * qos_overrides: + * /my/topic/name: + * publisher: # publisher without provided id + * reliability: reliable + * depth: 100 + * publisher_my_id: # publisher with `id="my_id" + * reliability: reliable + * depth: 10 + * ``` + */ +class QosOverridingOptions +{ +public: + /// Default constructor, no overrides allowed. + RCLCPP_PUBLIC + QosOverridingOptions() = default; + + /// Construct passing a list of QoS policies and a verification callback. + /** + * This constructor is implicit, e.g.: + * ```cpp + * node->create_publisher( + * "topic_name", + * default_qos_profile, + * { + * {QosPolicyKind::Reliability}, + * [] (auto && qos) {return check_qos_validity(qos)}, + * "my_id" + * }); + * ``` + * \param policy_kinds list of policy kinds that will be reconfigurable. + * \param validation_callback callbak that will be called to validate the validity of + * the qos profile set by the user. + * \param id id of the entity. + */ + RCLCPP_PUBLIC + QosOverridingOptions( + std::initializer_list policy_kinds, + QosCallback validation_callback = nullptr, + std::string id = {}); + + RCLCPP_PUBLIC + const std::string & + get_id() const; + + RCLCPP_PUBLIC + const std::vector & + get_policy_kinds() const; + + RCLCPP_PUBLIC + const QosCallback & + get_validation_callback() const; + + /// Construct passing a list of QoS policies and a verification callback. + /** + * Same as `QosOverridingOptions` constructor, but only declares the default policies: + * + * History, Depth, Reliability. + */ + RCLCPP_PUBLIC + static + QosOverridingOptions + with_default_policies(QosCallback validation_callback = nullptr, std::string id = {}); + +private: + /// \internal Id of the entity requesting to create parameters. + std::string id_; + /// \internal Policy kinds that are allowed to be reconfigured. + std::vector policy_kinds_; + /// \internal Validation callback that will be called to verify the profile. + QosCallback validation_callback_; +}; + +} // namespace rclcpp + +#endif // RCLCPP__QOS_OVERRIDING_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index ebf4331c4f..c3d509698d 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -26,6 +26,7 @@ #include "rclcpp/intra_process_setting.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" +#include "rclcpp/qos_overriding_options.hpp" #include "rclcpp/topic_statistics_state.hpp" #include "rclcpp/visibility_control.hpp" @@ -72,6 +73,8 @@ struct SubscriptionOptionsBase }; TopicStatisticsOptions topic_stats_options; + + QosOverridingOptions qos_overriding_options; }; /// Structure containing optional configuration for Subscriptions. diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index fba6e84f9d..8fa46abe60 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -70,6 +70,7 @@ NodeBase::NodeBase( std::shared_ptr logging_mutex = get_global_logging_mutex(); { std::lock_guard guard(*logging_mutex); + // TODO(ivanpauno): /rosout Qos should be reconfigurable. // TODO(ivanpauno): Instead of mutually excluding rcl_node_init with the global logger mutex, // rcl_logging_rosout_init_publisher_for_node could be decoupled from there and be called // here directly. diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 90c851be90..7560e31e30 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -67,6 +67,7 @@ NodeParameters::NodeParameters( } if (start_parameter_event_publisher) { + // TODO(ivanpauno): Qos of the `/parameters_event` topic should be somehow overridable. events_publisher_ = rclcpp::create_publisher( node_topics, "/parameter_events", diff --git a/rclcpp/src/rclcpp/qos_overriding_options.cpp b/rclcpp/src/rclcpp/qos_overriding_options.cpp new file mode 100644 index 0000000000..ab5d705bf3 --- /dev/null +++ b/rclcpp/src/rclcpp/qos_overriding_options.cpp @@ -0,0 +1,84 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rclcpp/qos_overriding_options.hpp" + +#include +#include +#include +#include +#include +#include + +#include "rmw/qos_policy_kind.h" +#include "rmw/qos_string_conversions.h" + +namespace rclcpp +{ + +const char * +qos_policy_kind_to_cstr(const QosPolicyKind & qpk) +{ + const char * ret = rmw_qos_policy_kind_to_str(static_cast(qpk)); + if (!ret) { + throw std::invalid_argument{"unknown QoS policy kind"}; + } + return ret; +} + +std::ostream & +operator<<(std::ostream & oss, const QosPolicyKind & qpk) +{ + return oss << qos_policy_kind_to_cstr(qpk); +} + +static std::initializer_list kDefaultPolicies = +{QosPolicyKind::History, QosPolicyKind::Depth, QosPolicyKind::Reliability}; + +QosOverridingOptions::QosOverridingOptions( + std::initializer_list policy_kinds, + QosCallback validation_callback, + std::string id) +: id_{std::move(id)}, + policy_kinds_{policy_kinds}, + validation_callback_{std::move(validation_callback)} +{} + +QosOverridingOptions +QosOverridingOptions::with_default_policies( + QosCallback validation_callback, + std::string id) +{ + return QosOverridingOptions{kDefaultPolicies, validation_callback, id}; +} + +const std::string & +QosOverridingOptions::get_id() const +{ + return id_; +} + +const std::vector & +QosOverridingOptions::get_policy_kinds() const +{ + return policy_kinds_; +} + +const QosCallback & +QosOverridingOptions::get_validation_callback() const +{ + return validation_callback_; +} + +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 304661e310..a1005836a0 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -369,6 +369,18 @@ if(TARGET test_qos_event) mimick ) endif() +ament_add_gmock(test_qos_overriding_options test_qos_overriding_options.cpp) +if(TARGET test_qos_overriding_options) + target_link_libraries(test_qos_overriding_options + ${PROJECT_NAME} + ) +endif() +ament_add_gmock(test_qos_parameters test_qos_parameters.cpp) +if(TARGET test_qos_parameters) + target_link_libraries(test_qos_parameters + ${PROJECT_NAME} + ) +endif() ament_add_gtest(test_rate test_rate.cpp) if(TARGET test_rate) ament_target_dependencies(test_rate diff --git a/rclcpp/test/rclcpp/test_create_subscription.cpp b/rclcpp/test/rclcpp/test_create_subscription.cpp index 184904b8e7..401e6c33ed 100644 --- a/rclcpp/test/rclcpp/test_create_subscription.cpp +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -50,6 +50,34 @@ TEST_F(TestCreateSubscription, create) { EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); } +TEST_F(TestCreateSubscription, create_with_overriding_options) { + auto node = std::make_shared("my_node", "/ns"); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + auto subscription = + rclcpp::create_subscription(node, "topic_name", qos, callback, options); + + ASSERT_NE(nullptr, subscription); + EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); +} + +TEST_F(TestCreateSubscription, create_separated_node_topics_and_parameters) { + auto node = std::make_shared("my_node", "/ns"); + const rclcpp::QoS qos(10); + auto options = rclcpp::SubscriptionOptions(); + auto callback = [](const test_msgs::msg::Empty::SharedPtr) {}; + + auto node_parameters = node->get_node_parameters_interface(); + auto node_topics = node->get_node_topics_interface(); + auto subscription = rclcpp::create_subscription( + node_parameters, node_topics, "topic_name", qos, callback, options); + + ASSERT_NE(nullptr, subscription); + EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); +} + TEST_F(TestCreateSubscription, create_with_statistics) { auto node = std::make_shared("my_node", "/ns"); const rclcpp::QoS qos(10); diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 15390bd274..e839788723 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -151,6 +151,20 @@ TEST_F(TestPublisher, various_creation_signatures) { rclcpp::create_publisher(node, "topic", 42, rclcpp::PublisherOptions()); (void)publisher; } + { + rclcpp::PublisherOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); + auto publisher = + rclcpp::create_publisher(node, "topic", 42, options); + (void)publisher; + } + { + auto node_parameters = node->get_node_parameters_interface(); + auto node_topics = node->get_node_topics_interface(); + auto publisher = rclcpp::create_publisher( + node_parameters, node_topics, "topic", 42, rclcpp::PublisherOptions()); + (void)publisher; + } } /* diff --git a/rclcpp/test/rclcpp/test_qos_overriding_options.cpp b/rclcpp/test/rclcpp/test_qos_overriding_options.cpp new file mode 100644 index 0000000000..f15fbe43ff --- /dev/null +++ b/rclcpp/test/rclcpp/test_qos_overriding_options.cpp @@ -0,0 +1,34 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gmock/gmock.h" + +#include "rclcpp/qos_overriding_options.hpp" + +TEST(TestQosOverridingOptions, test_overriding_options) { + auto options = rclcpp::QosOverridingOptions::with_default_policies(); + EXPECT_EQ(options.get_id(), ""); + EXPECT_EQ(options.get_validation_callback(), nullptr); + EXPECT_THAT( + options.get_policy_kinds(), testing::ElementsAre( + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Reliability)); +} + +TEST(TestQosOverridingOptions, test_qos_policy_kind_to_cstr) { + EXPECT_THROW( + rclcpp::qos_policy_kind_to_cstr(rclcpp::QosPolicyKind::Invalid), + std::invalid_argument); +} diff --git a/rclcpp/test/rclcpp/test_qos_parameters.cpp b/rclcpp/test/rclcpp/test_qos_parameters.cpp new file mode 100644 index 0000000000..c37b9a825c --- /dev/null +++ b/rclcpp/test/rclcpp/test_qos_parameters.cpp @@ -0,0 +1,256 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "gmock/gmock.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/qos_overriding_options.hpp" +#include "rclcpp/detail/qos_parameters.hpp" + +TEST(TestQosParameters, declare) { + rclcpp::init(0, nullptr); + auto node = std::make_shared( + "my_node", "/ns", rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter( + "qos_overrides./my/fully/qualified/topic_name.publisher.reliability", "best_effort"), + })); + + for (size_t i = 0; i < 2; ++i) { + // The first iteration will declare parameters, the second will get the previosuly declared + // ones, check both have the same result. + rclcpp::QoS qos{rclcpp::KeepLast(10)}; + qos = rclcpp::detail::declare_qos_parameters( + rclcpp::QosOverridingOptions::with_default_policies(), + node, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::PublisherQosParametersTraits{}); + + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.publisher.history").get_value(), + "keep_last"); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.publisher.depth").get_value(), + 10); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.publisher.reliability" + ).get_value(), + "best_effort"); + EXPECT_EQ(RMW_QOS_POLICY_HISTORY_KEEP_LAST, qos.get_rmw_qos_profile().history); + EXPECT_EQ(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, qos.get_rmw_qos_profile().reliability); + EXPECT_EQ(10u, qos.get_rmw_qos_profile().depth); + + std::map qos_params; + EXPECT_TRUE( + node->get_node_parameters_interface()->get_parameters_by_prefix( + "qos_overrides./my/fully/qualified/topic_name.publisher", qos_params)); + EXPECT_EQ(3u, qos_params.size()); + } + + rclcpp::shutdown(); +} + +TEST(TestQosParameters, declare_with_callback) { + rclcpp::init(0, nullptr); + auto node = std::make_shared( + "my_node", "/ns", rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter( + "qos_overrides./my/fully/qualified/topic_name.publisher.reliability", "best_effort"), + })); + + rclcpp::QoS qos{rclcpp::KeepLast(10)}; + // *INDENT-OFF*, uncrustify suggestion makes the code unreadable + EXPECT_THROW( + rclcpp::detail::declare_qos_parameters( + { + {rclcpp::QosPolicyKind::Lifespan}, + [](const rclcpp::QoS &) { + return rclcpp::QosCallbackResult{}; + } + }, + node, + "/my/fully/qualified/topic_name/fails_validation", + qos, + rclcpp::detail::PublisherQosParametersTraits{}), + rclcpp::exceptions::InvalidQosOverridesException); + + rclcpp::detail::declare_qos_parameters( + rclcpp::QosOverridingOptions::with_default_policies([](const rclcpp::QoS &) { + rclcpp::QosCallbackResult result; + result.successful = true; + return result; + }), + node, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::PublisherQosParametersTraits{}); + // *INDENT-ON* + + rclcpp::shutdown(); +} + +constexpr int64_t kDuration{1000000}; + +TEST(TestQosParameters, declare_qos_subscription_parameters) { + rclcpp::init(0, nullptr); + auto node = std::make_shared( + "my_node", "/ns", rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.reliability", "best_effort"), + rclcpp::Parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.deadline", kDuration), + rclcpp::Parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.liveliness_lease_duration", + kDuration), + })); + + for (size_t i = 0; i < 2; ++i) { + // The first iteration will declare parameters, the second will get the previosuly declared + // ones, check both have the same result. + rclcpp::QoS qos{rclcpp::KeepLast(10)}; + qos = rclcpp::detail::declare_qos_parameters( + { + rclcpp::QosPolicyKind::AvoidRosNamespaceConventions, rclcpp::QosPolicyKind::Deadline, + rclcpp::QosPolicyKind::Depth, rclcpp::QosPolicyKind::Durability, + // lifespan will be ignored + rclcpp::QosPolicyKind::History, rclcpp::QosPolicyKind::Lifespan, + rclcpp::QosPolicyKind::Liveliness, rclcpp::QosPolicyKind::LivelinessLeaseDuration, + rclcpp::QosPolicyKind::Reliability + }, + node, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::SubscriptionQosParametersTraits{}); + + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.avoid_ros_namespace_conventions" + ).get_value(), false); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.deadline" + ).get_value(), kDuration); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.depth" + ).get_value(), 10); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.durability" + ).get_value(), "volatile"); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.history" + ).get_value(), "keep_last"); + EXPECT_FALSE( + node->has_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.lifespan")); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.liveliness" + ).get_value(), "system_default"); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.liveliness_lease_duration" + ).get_value(), kDuration); + EXPECT_EQ( + node->get_parameter( + "qos_overrides./my/fully/qualified/topic_name.subscription.reliability" + ).get_value(), + "best_effort"); + + std::map qos_params; + EXPECT_TRUE( + node->get_node_parameters_interface()->get_parameters_by_prefix( + "qos_overrides./my/fully/qualified/topic_name.subscription", qos_params)); + EXPECT_EQ(8u, qos_params.size()); + } + rclcpp::shutdown(); +} + +TEST(TestQosParameters, declare_with_id) { + rclcpp::init(0, nullptr); + auto node = std::make_shared("my_node", "/ns"); + + rclcpp::QoS qos{rclcpp::KeepLast{10}}; + qos = rclcpp::detail::declare_qos_parameters( + rclcpp::QosOverridingOptions::with_default_policies(nullptr, "my_id"), + node, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::PublisherQosParametersTraits{}); + + std::map qos_params; + EXPECT_TRUE( + node->get_node_parameters_interface()->get_parameters_by_prefix( + "qos_overrides./my/fully/qualified/topic_name.publisher_my_id", qos_params)); + EXPECT_EQ(3u, qos_params.size()); + + rclcpp::shutdown(); +} + +TEST(TestQosParameters, declare_no_parameters_interface) { + rclcpp::init(0, nullptr); + auto node = std::make_shared("my_node", "/ns"); + + rclcpp::QoS qos{rclcpp::KeepLast{10}}; + auto node_base_interface = node->get_node_base_interface(); + EXPECT_THROW( + rclcpp::detail::declare_qos_parameters( + rclcpp::QosOverridingOptions::with_default_policies(), + node_base_interface, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::PublisherQosParametersTraits{}), + std::runtime_error); + + qos = rclcpp::detail::declare_qos_parameters( + rclcpp::QosOverridingOptions{}, + node_base_interface, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::PublisherQosParametersTraits{}); + + std::map qos_params; + EXPECT_FALSE( + node->get_node_parameters_interface()->get_parameters_by_prefix("qos_overrides", qos_params)); + + rclcpp::shutdown(); +} + +TEST(TestQosParameters, internal_functions_failure_modes) { + rclcpp::QoS qos{rclcpp::KeepLast{10}}; + EXPECT_THROW( + rclcpp::detail::apply_qos_override( + rclcpp::QosPolicyKind::Invalid, rclcpp::ParameterValue{}, qos), + std::invalid_argument); + EXPECT_THROW( + rclcpp::detail::get_default_qos_param_value( + rclcpp::QosPolicyKind::Invalid, qos), + std::invalid_argument); + EXPECT_THROW( + rclcpp::detail::check_if_stringified_policy_is_null( + nullptr, rclcpp::QosPolicyKind::Reliability), + std::invalid_argument); +}