From c68112c1118241448fdc03eb1f380b952d0cc7f7 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 15 Oct 2020 10:09:11 -0300 Subject: [PATCH 01/38] refactor + tests Signed-off-by: Ivan Santiago Paunovic --- rclcpp/CMakeLists.txt | 1 + .../include/rclcpp/detail/qos_parameters.hpp | 431 ++++++++++++++++++ .../include/rclcpp/exceptions/exceptions.hpp | 7 + rclcpp/include/rclcpp/node.hpp | 24 +- .../include/rclcpp/qos_overriding_options.hpp | 137 ++++++ rclcpp/src/rclcpp/qos_overriding_options.cpp | 92 ++++ rclcpp/test/rclcpp/CMakeLists.txt | 6 + .../rclcpp/detail/test_qos_parameters.cpp | 78 ++++ 8 files changed, 766 insertions(+), 10 deletions(-) create mode 100644 rclcpp/include/rclcpp/detail/qos_parameters.hpp create mode 100644 rclcpp/include/rclcpp/qos_overriding_options.hpp create mode 100644 rclcpp/src/rclcpp/qos_overriding_options.cpp create mode 100644 rclcpp/test/rclcpp/detail/test_qos_parameters.cpp 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/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp new file mode 100644 index 0000000000..48faf5fe34 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -0,0 +1,431 @@ +// 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 "rcl_interfaces/msg/parameter_descriptor.hpp" + +#include "rclcpp/duration.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::HistoryDepth, + 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::HistoryDepth, + QosPolicyKind::Liveliness, + QosPolicyKind::LivelinessLeaseDuration, + QosPolicyKind::Reliability, + }; + } +}; + +/// \internal Declare qos parameters for the given entity. +/** + * \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 parameters_interface Parameters will be declared through this interface. + * \param default_id TODO change this to topic name. + * \param qos User provided qos. It will be used as a default for the parameters declared, + * and then overriden with the final parameter overrides. + */ +template +inline +void +declare_qos_parameters( + const ::rclcpp::QosOverridingOptions & options, + ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, + const std::string & default_id, + ::rclcpp::QoS & qos, + EntityQosParametersTraits); + +/// \internal Same as `declare_qos_parameters()` for a `Publisher`. +inline +void +declare_publisher_qos_parameters( + const ::rclcpp::QosOverridingOptions & options, + ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, + const std::string & default_id, + ::rclcpp::QoS & qos) +{ + declare_qos_parameters( + options, parameters_interface, default_id, qos, PublisherQosParametersTraits{}); +} + +/// \internal Same as `declare_qos_parameters()` for a `Subscription`. +inline +void +declare_subscription_qos_parameters( + const ::rclcpp::QosOverridingOptions & options, + ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, + const std::string & default_id, + ::rclcpp::QoS & qos) +{ + declare_qos_parameters( + options, parameters_interface, default_id, qos, SubscriptionQosParametersTraits{}); +} + +/// \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); + +template +inline +void +declare_qos_parameters( + const ::rclcpp::QosOverridingOptions & options, + ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, + const std::string & default_id, + ::rclcpp::QoS & qos, + EntityQosParametersTraits) +{ + const auto & id = options.id.empty() ? default_id : options.id; + for (auto policy : EntityQosParametersTraits::allowed_policies()) { + if ( + std::count(options.qos_policy_kinds.begin(), options.qos_policy_kinds.end(), policy)) + { + std::ostringstream param_name{"qos_profiles.", std::ios::ate}; + param_name << + EntityQosParametersTraits::entity_type() << "." << id << "." << + qos_policy_kind_to_cstr(policy); + std::ostringstream param_desciption{"qos policy {", std::ios::ate}; + param_desciption << qos_policy_kind_to_cstr(policy) << "} for {" << id << "}"; + rcl_interfaces::msg::ParameterDescriptor descriptor{}; + descriptor.description = param_desciption.str(); + descriptor.read_only = true; + auto value = parameters_interface.declare_parameter( + param_name.str(), get_default_qos_param_value(policy, qos), descriptor); + ::rclcpp::detail::apply_qos_override(policy, value, qos); + } + } + if (options.validation_callback && !options.validation_callback(qos)) { + throw rclcpp::exceptions::InvalidQosOverridesException{"validation callback failed"}; + } +} + +/// \internal Get the `rmw_qos_*_policy_t` value from a given `str`, or raise a runtime_error. +template +RetT +string_to_policy(const std::string & str); + +template<> +inline +rmw_qos_durability_policy_t +string_to_policy(const std::string & str); + +template<> +inline +rmw_qos_liveliness_policy_t +string_to_policy(const std::string & str); + +template<> +inline +rmw_qos_history_policy_t +string_to_policy(const std::string & str); + +template<> +inline +rmw_qos_reliability_policy_t +string_to_policy(const std::string & str); + +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(value.get())); + break; + case QosPolicyKind::Durability: + qos.durability(string_to_policy(value.get())); + break; + case QosPolicyKind::History: + qos.history(string_to_policy(value.get())); + break; + case QosPolicyKind::HistoryDepth: + qos.get_rmw_qos_profile().depth = static_cast(value.get()); + break; + case QosPolicyKind::Lifespan: + qos.lifespan(::rclcpp::Duration(value.get())); + break; + case QosPolicyKind::Liveliness: + qos.liveliness(string_to_policy(value.get())); + break; + case QosPolicyKind::LivelinessLeaseDuration: + qos.liveliness_lease_duration(::rclcpp::Duration(value.get())); + break; + case QosPolicyKind::Reliability: + qos.reliability(string_to_policy(value.get())); + break; + default: + throw std::runtime_error{"unknown QosPolicyKind"}; + } +} + +/// Convert the given policy to the corresponding string representation. +inline +const char * +policy_to_cstring(rmw_qos_durability_policy_t durability); + +/// Convert the given policy to the corresponding string representation. +inline +const char * +policy_to_cstring(rmw_qos_history_policy_t history); + +/// Convert the given policy to the corresponding string representation. +inline +const char * +policy_to_cstring(rmw_qos_liveliness_policy_t liveliness); + +/// Convert the given policy to the corresponding string representation. +inline +const char * +policy_to_cstring(rmw_qos_reliability_policy_t reliability); + +/// 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(); +} + +inline +::rclcpp::ParameterValue +get_default_qos_param_value(rclcpp::QosPolicyKind qpk, const rclcpp::QoS & qos) +{ + using ParameterValue = ::rclcpp::ParameterValue; + const auto & rmw_qos = qos.get_rmw_qos_profile(); + switch (qpk) { + 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(policy_to_cstring(rmw_qos.durability)); + case QosPolicyKind::History: + return ParameterValue(policy_to_cstring(rmw_qos.history)); + case QosPolicyKind::HistoryDepth: + 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(policy_to_cstring(rmw_qos.liveliness)); + case QosPolicyKind::LivelinessLeaseDuration: + return ParameterValue(rmw_duration_to_int64_t(rmw_qos.liveliness_lease_duration)); + case QosPolicyKind::Reliability: + return ParameterValue(policy_to_cstring(rmw_qos.reliability)); + default: + throw std::invalid_argument{"unknown qos policy kind"}; + } +} + +// TODO(ivanpauno): All `policy_to_cstring()` and `string_to_policy()` functions should be +// a wrapper of a `rcl` implemented function. +inline +const char * +policy_to_cstring(rmw_qos_durability_policy_t durability) +{ + switch (durability) { + case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: + return "system_default"; + case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: + return "transient_local"; + case RMW_QOS_POLICY_DURABILITY_VOLATILE: + return "volatile"; + case RMW_QOS_POLICY_DURABILITY_UNKNOWN: // fallthrough + default: + throw std::invalid_argument{"unknown durability qos policy value"}; + } +} + +inline +const char * +policy_to_cstring(rmw_qos_history_policy_t history) +{ + switch (history) { + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + return "system_default"; + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + return "keep_last"; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + return "keep_all"; + case RMW_QOS_POLICY_HISTORY_UNKNOWN: // fallthrough + default: + throw std::invalid_argument{"unknown history qos policy value"}; + } +} + +inline +const char * +policy_to_cstring(rmw_qos_liveliness_policy_t liveliness) +{ + switch (liveliness) { + case RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT: + return "system_default"; + case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: + return "automatic"; + case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: + return "manual_by_topic"; + case RMW_QOS_POLICY_LIVELINESS_UNKNOWN: // fallthrough + default: + throw std::invalid_argument{"unknown liveliness qos policy value"}; + } +} + +inline +const char * +policy_to_cstring(rmw_qos_reliability_policy_t reliability) +{ + switch (reliability) { + case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: + return "system_default"; + case RMW_QOS_POLICY_RELIABILITY_RELIABLE: + return "reliable"; + case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: + return "best_effort"; + case RMW_QOS_POLICY_RELIABILITY_UNKNOWN: // fallthrough + default: + throw std::invalid_argument{"unknown reliability qos policy value"}; + } +} + +template<> +inline +rmw_qos_durability_policy_t +string_to_policy(const std::string & str) +{ + if ("system_default" == str) { + return RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT; + } + if ("transient_local" == str) { + return RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + } + if ("volatile" == str) { + return RMW_QOS_POLICY_DURABILITY_VOLATILE; + } + throw std::invalid_argument{"unknown durability qos policy string"}; +} + +template<> +inline +rmw_qos_history_policy_t +string_to_policy(const std::string & str) +{ + if ("system_default" == str) { + return RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT; + } + if ("keep_last" == str) { + return RMW_QOS_POLICY_HISTORY_KEEP_LAST; + } + if ("keep_all" == str) { + return RMW_QOS_POLICY_HISTORY_KEEP_ALL; + } + throw std::invalid_argument{"unknown history qos policy string"}; +} + +template<> +inline +rmw_qos_liveliness_policy_t +string_to_policy(const std::string & str) +{ + if ("system_default" == str) { + return RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; + } + if ("automatic" == str) { + return RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + } + if ("manual_by_topic" == str) { + return RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + } + throw std::invalid_argument{"unknown liveliness qos policy string"}; +} + +template<> +inline +rmw_qos_reliability_policy_t +string_to_policy(const std::string & str) +{ + if ("system_default" == str) { + return RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; + } + if ("reliable" == str) { + return RMW_QOS_POLICY_RELIABILITY_RELIABLE; + } + if ("best_effort" == str) { + return RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + } + throw std::invalid_argument{"unknown reliability qos policy string"}; +} + +} // 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/node.hpp b/rclcpp/include/rclcpp/node.hpp index f2247b59a1..f4ea66e8be 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -44,21 +44,12 @@ #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" -#include "rclcpp/node_interfaces/node_base_interface.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" -#include "rclcpp/node_interfaces/node_graph_interface.hpp" -#include "rclcpp/node_interfaces/node_logging_interface.hpp" -#include "rclcpp/node_interfaces/node_parameters_interface.hpp" -#include "rclcpp/node_interfaces/node_services_interface.hpp" -#include "rclcpp/node_interfaces/node_time_source_interface.hpp" -#include "rclcpp/node_interfaces/node_timers_interface.hpp" -#include "rclcpp/node_interfaces/node_topics_interface.hpp" -#include "rclcpp/node_interfaces/node_waitables_interface.hpp" #include "rclcpp/node_options.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp/publisher_options.hpp" #include "rclcpp/qos.hpp" +#include "rclcpp/qos_overriding_options.hpp" #include "rclcpp/service.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/subscription_options.hpp" @@ -67,6 +58,19 @@ #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/detail/qos_parameters.hpp" + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" + namespace rclcpp { diff --git a/rclcpp/include/rclcpp/qos_overriding_options.hpp b/rclcpp/include/rclcpp/qos_overriding_options.hpp new file mode 100644 index 0000000000..4c189961e7 --- /dev/null +++ b/rclcpp/include/rclcpp/qos_overriding_options.hpp @@ -0,0 +1,137 @@ +// 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" + +namespace rclcpp +{ + +enum class QosPolicyKind +{ + AvoidRosNamespaceConventions, + Deadline, + Durability, + History, + HistoryDepth, + Lifespan, + Liveliness, + LivelinessLeaseDuration, + Reliability, +}; + +const char * +qos_policy_kind_to_cstr(const QosPolicyKind & qpk); + +std::ostream & +operator<<(std::ostream & os, const QosPolicyKind & qpk); + +using QosCallback = std::function; + +namespace detail +{ +// forward declare +template +class QosParameters; +} + +/// Options that are passed in subscription/publisher constructor to specify QoSConfigurability. +/** + * TODO: Write nice docs here. + */ +struct QosOverridingOptions +{ + /// Id of the entity requesting to create parameters. + std::string id; + /// Policy kinds that are allowed to be reconfigured. + std::vector qos_policy_kinds; + /// Validation callback that will be called to verify the profile. + QosCallback validation_callback; + + /// Construct using default overriding options. + /** + * \param declare_default_parameters if `true`, the default set of qos that can be + * reconfigured will be declared. If `false`, qos aren't reconfigurable. + * \param id id of the entity. + */ + explicit QosOverridingOptions(bool declare_default_parameters = false, std::string id = {}); + + /// Construct passing a list of qos policies that can be overriden. + /** + * This constructor is implicit, e.g.: + * ```cpp + * node->create_publisher( + * "topic_name", + * default_qos_profile, + * {{QosPolicyKind::Reliability}, "my_id"}); + * ``` + * \param policy_kinds list of policy kinds that will be reconfigurable. + * \param id id of the entity. + */ + + QosOverridingOptions(std::initializer_list policy_kinds, std::string id = {}); + /// Construct passing a list of qos policies that 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. + */ + QosOverridingOptions( + std::initializer_list policy_kinds, + QosCallback validation_callback, + std::string id = {}); + + /// Construct using default overriding options and passing a validation callback. + /** + * This constructor is implicit, e.g.: + * ```cpp + * node->create_publisher( + * "topic_name", + * default_qos_profile, + * { + * [] (auto && qos) {return check_qos_validity(qos)}, + * "my_id" + * }); + * ``` + * \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. + */ + QosOverridingOptions(QosCallback validation_callback, std::string id = {}); // NOLINT, implicit +}; + +} // namespace rclcpp + +#endif // RCLCPP__QOS_OVERRIDING_OPTIONS_HPP_ diff --git a/rclcpp/src/rclcpp/qos_overriding_options.cpp b/rclcpp/src/rclcpp/qos_overriding_options.cpp new file mode 100644 index 0000000000..413d771321 --- /dev/null +++ b/rclcpp/src/rclcpp/qos_overriding_options.cpp @@ -0,0 +1,92 @@ +// 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 + +namespace rclcpp +{ + +const char * +qos_policy_kind_to_cstr(const QosPolicyKind & qpk) +{ + switch (qpk) { + case QosPolicyKind::AvoidRosNamespaceConventions: + return "avoid_ros_namespace_conventions"; + case QosPolicyKind::Deadline: + return "deadline"; + case QosPolicyKind::Durability: + return "durability"; + case QosPolicyKind::History: + return "history"; + case QosPolicyKind::HistoryDepth: + return "history_depth"; + case QosPolicyKind::Lifespan: + return "lifespan"; + case QosPolicyKind::Liveliness: + return "liveliness"; + case QosPolicyKind::LivelinessLeaseDuration: + return "liveliness_lease_duration"; + case QosPolicyKind::Reliability: + return "reliability"; + default: + throw std::invalid_argument{"unknown qos policy kind"}; + } +} + +std::ostream & +operator<<(std::ostream & oss, const QosPolicyKind & qpk) +{ + return oss << qos_policy_kind_to_cstr(qpk); +} + +#define RCLCPP_QOS_OVERRIDING_OPTIONS_DEFAULT_QOS_POLICIES (std::initializer_list \ + {QosPolicyKind::History, QosPolicyKind::HistoryDepth, QosPolicyKind::Reliability}) + +QosOverridingOptions::QosOverridingOptions(bool declare_default_parameters, std::string id) +: id{std::move(id)}, + qos_policy_kinds{declare_default_parameters ? + RCLCPP_QOS_OVERRIDING_OPTIONS_DEFAULT_QOS_POLICIES : + std::initializer_list{}} +{} + +QosOverridingOptions::QosOverridingOptions( + std::initializer_list policy_kinds, std::string id) +: id{std::move(id)}, + qos_policy_kinds{policy_kinds} +{} + +QosOverridingOptions::QosOverridingOptions( + std::initializer_list policy_kinds, + QosCallback validation_callback, + std::string id) +: id{std::move(id)}, + qos_policy_kinds{policy_kinds}, + validation_callback{std::move(validation_callback)} +{} + +QosOverridingOptions::QosOverridingOptions( + QosCallback validation_callback, + std::string id) +: id{std::move(id)}, + qos_policy_kinds{RCLCPP_QOS_OVERRIDING_OPTIONS_DEFAULT_QOS_POLICIES}, + validation_callback{std::move(validation_callback)} +{} + +} // namespace rclcpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 304661e310..0e7ca5724d 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -369,6 +369,12 @@ if(TARGET test_qos_event) mimick ) endif() +ament_add_gmock(test_qos_parameters detail/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/detail/test_qos_parameters.cpp b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp new file mode 100644 index 0000000000..5f5df4308a --- /dev/null +++ b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp @@ -0,0 +1,78 @@ +// 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, test_overriding_options) { + { + rclcpp::QosOverridingOptions options{true}; + EXPECT_EQ(options.id, ""); + EXPECT_EQ(options.validation_callback, nullptr); + EXPECT_THAT( + options.qos_policy_kinds, testing::ElementsAre( + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::HistoryDepth, + rclcpp::QosPolicyKind::Reliability)); + } +} + +TEST(TestQosParameters, declare) { + rclcpp::init(0, nullptr); + auto node = std::make_shared( + "my_node", "/ns", rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter( + "qos_profiles.publisher./my/fully/qualified/topic_name.reliability", "best_effort"), + })); + + rclcpp::QoS qos{rclcpp::KeepLast(10)}; + rclcpp::detail::declare_publisher_qos_parameters( + rclcpp::QosOverridingOptions{true}, + *node->get_node_parameters_interface(), + "/my/fully/qualified/topic_name", + qos); + + EXPECT_EQ( + node->get_parameter( + "qos_profiles.publisher./my/fully/qualified/topic_name.history").get_value(), + "keep_last"); + EXPECT_EQ( + node->get_parameter( + "qos_profiles.publisher./my/fully/qualified/topic_name.history_depth").get_value(), + 10); + EXPECT_EQ( + node->get_parameter( + "qos_profiles.publisher./my/fully/qualified/topic_name.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_profiles.publisher./my/fully/qualified/topic_name", qos_params)); + EXPECT_EQ(3u, qos_params.size()); + + rclcpp::shutdown(); +} From 5a1c5c7eb034f96adc5bfcc4d7d508aa47f675a1 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 15 Oct 2020 15:44:15 -0300 Subject: [PATCH 02/38] Update create_publisher, and others ... Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_publisher.hpp | 58 ++++++++++++++++++- .../include/rclcpp/detail/qos_parameters.hpp | 54 +++++++++++------ rclcpp/include/rclcpp/publisher_options.hpp | 3 + .../include/rclcpp/qos_overriding_options.hpp | 2 +- rclcpp/src/rclcpp/qos_overriding_options.cpp | 9 +-- .../rclcpp/detail/test_qos_parameters.cpp | 12 ++-- 6 files changed, 108 insertions(+), 30 deletions(-) diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index 811c18b69f..e0999e8dbd 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -17,18 +17,70 @@ #include #include +#include +#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/node_options.hpp" #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 +{ +// This is needed because it's currently possible to call `create_publisher()` +// only with a node topics interface. +// In that case, passing qos overridding options doesn't make sense. +// +// List of places in rclcpp passing directly a topic interface: +// creating of "/parameters_event" topic in node parameters interface (this one is tricky). +// +// TODO(ivanpauno): Write overload in which you can pass +// a topic interface and a parameter interface directly. +// TODO2(ivanpauno): If we want the qos of `/parameters_event` topic to be reconfigurable, +// we need to figure out something. +template +std::enable_if_t::value, rclcpp::QoS> +get_actual_qos( + const rclcpp::QosOverridingOptions & options, NodeT & node, + std::string topic_name, rclcpp::QoS actual_qos) +{ + using rclcpp::node_interfaces::get_node_parameters_interface; + if (options.policy_kinds.size()) { + // TODO(ivanpauno) + // Get expanded and remapped topic name before creating the node. + // Need to refactor things in `rcl`. + detail::declare_publisher_qos_parameters( + options, + *get_node_parameters_interface(node), + topic_name, // this should be the expanded and remapped topic name + actual_qos); + } + return actual_qos; +} + +template +std::enable_if_t::value, rclcpp::QoS> +get_actual_qos( + const rclcpp::QosOverridingOptions & options, NodeT, std::string, rclcpp::QoS actual_qos) +{ + if (options.policy_kinds.size()) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "qos override options ignored because no parameter interface was provided"); + } + return actual_qos; +} +} // 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() @@ -53,11 +105,15 @@ create_publisher( using rclcpp::node_interfaces::get_node_topics_interface; auto node_topics = get_node_topics_interface(node); + using rclcpp::node_interfaces::get_node_parameters_interface; + rclcpp::QoS actual_qos = detail::get_actual_qos( + options.qos_overriding_options, node, topic_name, qos); + // Create the publisher. auto pub = node_topics->create_publisher( topic_name, rclcpp::create_publisher_factory(options), - qos + actual_qos ); // Add the publisher to the node topics interface. diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index 48faf5fe34..bff830e62f 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -35,8 +35,9 @@ namespace detail { /// \internal Trait used to specialize `declare_qos_parameters()` for publishers. -struct PublisherQosParametersTraits { - static constexpr const char * entity_type() { return "publisher"; } +struct PublisherQosParametersTraits +{ + static constexpr const char * entity_type() {return "publisher";} static constexpr auto allowed_policies() { return std::array<::rclcpp::QosPolicyKind, 9> { @@ -54,8 +55,9 @@ struct PublisherQosParametersTraits { }; /// \internal Trait used to specialize `declare_qos_parameters()` for subscriptions. -struct SubscriptionQosParametersTraits { - static constexpr const char * entity_type() { return "subscription"; } +struct SubscriptionQosParametersTraits +{ + static constexpr const char * entity_type() {return "subscription";} static constexpr auto allowed_policies() { return std::array<::rclcpp::QosPolicyKind, 8> { @@ -78,7 +80,7 @@ struct 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 parameters_interface Parameters will be declared through this interface. - * \param default_id TODO change this to topic name. + * \param topic_name Name of the topic of the entity. * \param qos User provided qos. It will be used as a default for the parameters declared, * and then overriden with the final parameter overrides. */ @@ -88,7 +90,7 @@ void declare_qos_parameters( const ::rclcpp::QosOverridingOptions & options, ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, - const std::string & default_id, + const std::string & topic_name, ::rclcpp::QoS & qos, EntityQosParametersTraits); @@ -98,11 +100,11 @@ void declare_publisher_qos_parameters( const ::rclcpp::QosOverridingOptions & options, ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, - const std::string & default_id, + const std::string & topic_name, ::rclcpp::QoS & qos) { declare_qos_parameters( - options, parameters_interface, default_id, qos, PublisherQosParametersTraits{}); + options, parameters_interface, topic_name, qos, PublisherQosParametersTraits{}); } /// \internal Same as `declare_qos_parameters()` for a `Subscription`. @@ -111,11 +113,11 @@ void declare_subscription_qos_parameters( const ::rclcpp::QosOverridingOptions & options, ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, - const std::string & default_id, + const std::string & topic_name, ::rclcpp::QoS & qos) { declare_qos_parameters( - options, parameters_interface, default_id, qos, SubscriptionQosParametersTraits{}); + options, parameters_interface, topic_name, qos, SubscriptionQosParametersTraits{}); } /// \internal Returns the given `policy` of the profile `qos` converted to a parameter value. @@ -135,21 +137,37 @@ void declare_qos_parameters( const ::rclcpp::QosOverridingOptions & options, ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, - const std::string & default_id, + const std::string & topic_name, ::rclcpp::QoS & qos, EntityQosParametersTraits) { - const auto & id = options.id.empty() ? default_id : options.id; + std::string param_prefix; + { + std::ostringstream oss{"qos_overrides.", std::ios::ate}; + oss << topic_name << "." << EntityQosParametersTraits::entity_type(); + if (!options.id.empty()) { + oss << "_" << options.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 (!options.id.empty()) { + oss << " with id {" << options.id << "}"; + } + param_description_suffix = oss.str(); + } for (auto policy : EntityQosParametersTraits::allowed_policies()) { if ( - std::count(options.qos_policy_kinds.begin(), options.qos_policy_kinds.end(), policy)) + std::count(options.policy_kinds.begin(), options.policy_kinds.end(), policy)) { - std::ostringstream param_name{"qos_profiles.", std::ios::ate}; - param_name << - EntityQosParametersTraits::entity_type() << "." << id << "." << - qos_policy_kind_to_cstr(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) << "} for {" << id << "}"; + 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; 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 index 4c189961e7..826965ea1b 100644 --- a/rclcpp/include/rclcpp/qos_overriding_options.hpp +++ b/rclcpp/include/rclcpp/qos_overriding_options.hpp @@ -64,7 +64,7 @@ struct QosOverridingOptions /// Id of the entity requesting to create parameters. std::string id; /// Policy kinds that are allowed to be reconfigured. - std::vector qos_policy_kinds; + std::vector policy_kinds; /// Validation callback that will be called to verify the profile. QosCallback validation_callback; diff --git a/rclcpp/src/rclcpp/qos_overriding_options.cpp b/rclcpp/src/rclcpp/qos_overriding_options.cpp index 413d771321..3adf89cb48 100644 --- a/rclcpp/src/rclcpp/qos_overriding_options.cpp +++ b/rclcpp/src/rclcpp/qos_overriding_options.cpp @@ -23,6 +23,7 @@ namespace rclcpp { +// TODO(ivanpauno): this should be wrapping an rcl function const char * qos_policy_kind_to_cstr(const QosPolicyKind & qpk) { @@ -61,7 +62,7 @@ operator<<(std::ostream & oss, const QosPolicyKind & qpk) QosOverridingOptions::QosOverridingOptions(bool declare_default_parameters, std::string id) : id{std::move(id)}, - qos_policy_kinds{declare_default_parameters ? + policy_kinds{declare_default_parameters ? RCLCPP_QOS_OVERRIDING_OPTIONS_DEFAULT_QOS_POLICIES : std::initializer_list{}} {} @@ -69,7 +70,7 @@ QosOverridingOptions::QosOverridingOptions(bool declare_default_parameters, std: QosOverridingOptions::QosOverridingOptions( std::initializer_list policy_kinds, std::string id) : id{std::move(id)}, - qos_policy_kinds{policy_kinds} + policy_kinds{policy_kinds} {} QosOverridingOptions::QosOverridingOptions( @@ -77,7 +78,7 @@ QosOverridingOptions::QosOverridingOptions( QosCallback validation_callback, std::string id) : id{std::move(id)}, - qos_policy_kinds{policy_kinds}, + policy_kinds{policy_kinds}, validation_callback{std::move(validation_callback)} {} @@ -85,7 +86,7 @@ QosOverridingOptions::QosOverridingOptions( QosCallback validation_callback, std::string id) : id{std::move(id)}, - qos_policy_kinds{RCLCPP_QOS_OVERRIDING_OPTIONS_DEFAULT_QOS_POLICIES}, + policy_kinds{RCLCPP_QOS_OVERRIDING_OPTIONS_DEFAULT_QOS_POLICIES}, validation_callback{std::move(validation_callback)} {} diff --git a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp index 5f5df4308a..aa9f4b83ca 100644 --- a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp @@ -28,7 +28,7 @@ TEST(TestQosParameters, test_overriding_options) { EXPECT_EQ(options.id, ""); EXPECT_EQ(options.validation_callback, nullptr); EXPECT_THAT( - options.qos_policy_kinds, testing::ElementsAre( + options.policy_kinds, testing::ElementsAre( rclcpp::QosPolicyKind::History, rclcpp::QosPolicyKind::HistoryDepth, rclcpp::QosPolicyKind::Reliability)); @@ -41,7 +41,7 @@ TEST(TestQosParameters, declare) { "my_node", "/ns", rclcpp::NodeOptions().parameter_overrides( { rclcpp::Parameter( - "qos_profiles.publisher./my/fully/qualified/topic_name.reliability", "best_effort"), + "qos_overrides./my/fully/qualified/topic_name.publisher.reliability", "best_effort"), })); rclcpp::QoS qos{rclcpp::KeepLast(10)}; @@ -53,15 +53,15 @@ TEST(TestQosParameters, declare) { EXPECT_EQ( node->get_parameter( - "qos_profiles.publisher./my/fully/qualified/topic_name.history").get_value(), + "qos_overrides./my/fully/qualified/topic_name.publisher.history").get_value(), "keep_last"); EXPECT_EQ( node->get_parameter( - "qos_profiles.publisher./my/fully/qualified/topic_name.history_depth").get_value(), + "qos_overrides./my/fully/qualified/topic_name.publisher.history_depth").get_value(), 10); EXPECT_EQ( node->get_parameter( - "qos_profiles.publisher./my/fully/qualified/topic_name.reliability" + "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); @@ -71,7 +71,7 @@ TEST(TestQosParameters, declare) { std::map qos_params; EXPECT_TRUE( node->get_node_parameters_interface()->get_parameters_by_prefix( - "qos_profiles.publisher./my/fully/qualified/topic_name", qos_params)); + "qos_overrides./my/fully/qualified/topic_name.publisher", qos_params)); EXPECT_EQ(3u, qos_params.size()); rclcpp::shutdown(); From 472a1bd6e58593e294b4a67cad7d391bc98b4b7f Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 15 Oct 2020 16:23:29 -0300 Subject: [PATCH 03/38] Add QosOverridingOptions to create_subscription Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_publisher.hpp | 9 ++-- rclcpp/include/rclcpp/create_subscription.hpp | 47 ++++++++++++++++++- .../include/rclcpp/subscription_options.hpp | 3 ++ 3 files changed, 53 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index e0999e8dbd..9da558bdcd 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -41,7 +41,8 @@ namespace detail // In that case, passing qos overridding options doesn't make sense. // // List of places in rclcpp passing directly a topic interface: -// creating of "/parameters_event" topic in node parameters interface (this one is tricky). +// creation of "/parameters_event" topic in node parameters interface (this one is tricky). +// creation of "topic statistics" in create_subscription // // TODO(ivanpauno): Write overload in which you can pass // a topic interface and a parameter interface directly. @@ -49,7 +50,7 @@ namespace detail // we need to figure out something. template std::enable_if_t::value, rclcpp::QoS> -get_actual_qos( +get_publisher_actual_qos( const rclcpp::QosOverridingOptions & options, NodeT & node, std::string topic_name, rclcpp::QoS actual_qos) { @@ -69,7 +70,7 @@ get_actual_qos( template std::enable_if_t::value, rclcpp::QoS> -get_actual_qos( +get_publisher_actual_qos( const rclcpp::QosOverridingOptions & options, NodeT, std::string, rclcpp::QoS actual_qos) { if (options.policy_kinds.size()) { @@ -106,7 +107,7 @@ create_publisher( auto node_topics = get_node_topics_interface(node); using rclcpp::node_interfaces::get_node_parameters_interface; - rclcpp::QoS actual_qos = detail::get_actual_qos( + rclcpp::QoS actual_qos = detail::get_publisher_actual_qos( options.qos_overriding_options, node, topic_name, qos); // Create the publisher. diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 9c22d171d1..7ae2bab7c2 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -41,6 +41,46 @@ namespace rclcpp { +namespace detail +{ +// See comment in `get_subscription_actual_qos`. +// +// List of places in rclcpp passing directly a topic interface: +// creation of "/clock" topic in time source (can be solved with an overload accepting a parameters interface). +template +std::enable_if_t::value, rclcpp::QoS> +get_subscription_actual_qos( + const rclcpp::QosOverridingOptions & options, NodeT & node, + std::string topic_name, rclcpp::QoS actual_qos) +{ + using rclcpp::node_interfaces::get_node_parameters_interface; + if (options.policy_kinds.size()) { + // TODO(ivanpauno) + // Get expanded and remapped topic name before creating the node. + // Need to refactor things in `rcl`. + detail::declare_subscription_qos_parameters( + options, + *get_node_parameters_interface(node), + topic_name, // this should be the expanded and remapped topic name + actual_qos); + } + return actual_qos; +} + +template +std::enable_if_t::value, rclcpp::QoS> +get_subscription_actual_qos( + const rclcpp::QosOverridingOptions & options, NodeT, std::string, rclcpp::QoS actual_qos) +{ + if (options.policy_kinds.size()) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "qos override options ignored because no parameter interface was provided"); + } + return actual_qos; +} +} // 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() @@ -111,7 +151,7 @@ create_subscription( create_publisher( node, options.topic_stats_options.publish_topic, - qos); + qos); // why the topics statistics publisher is using the same QoS as the subscription?? subscription_topic_stats = std::make_shared< rclcpp::topic_statistics::SubscriptionTopicStatistics @@ -148,7 +188,10 @@ create_subscription( subscription_topic_stats ); - auto sub = node_topics->create_subscription(topic_name, factory, qos); + rclcpp::QoS actual_qos = detail::get_subscription_actual_qos( + options.qos_overriding_options, node, topic_name, qos); + + auto sub = node_topics->create_subscription(topic_name, factory, actual_qos); node_topics->add_subscription(sub, options.callback_group); return std::dynamic_pointer_cast(sub); 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. From dc1b0756942849fb549c25e3ff16efdf066c0a53 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 15 Oct 2020 16:53:00 -0300 Subject: [PATCH 04/38] linters Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_publisher.hpp | 2 +- rclcpp/include/rclcpp/create_subscription.hpp | 3 ++- rclcpp/include/rclcpp/detail/qos_parameters.hpp | 6 ++---- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index 9da558bdcd..92b6539e08 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -46,7 +46,7 @@ namespace detail // // TODO(ivanpauno): Write overload in which you can pass // a topic interface and a parameter interface directly. -// TODO2(ivanpauno): If we want the qos of `/parameters_event` topic to be reconfigurable, +// TODO(ivanpauno): If we want the qos of `/parameters_event` topic to be reconfigurable, // we need to figure out something. template std::enable_if_t::value, rclcpp::QoS> diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 7ae2bab7c2..7b83105952 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -46,7 +46,8 @@ namespace detail // See comment in `get_subscription_actual_qos`. // // List of places in rclcpp passing directly a topic interface: -// creation of "/clock" topic in time source (can be solved with an overload accepting a parameters interface). +// creation of "/clock" topic in time source +// (can be fixed by adding an overload accepting a parameters interface). template std::enable_if_t::value, rclcpp::QoS> get_subscription_actual_qos( diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index bff830e62f..10ff3c9857 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -86,8 +86,7 @@ struct SubscriptionQosParametersTraits */ template inline -void -declare_qos_parameters( +void declare_qos_parameters( const ::rclcpp::QosOverridingOptions & options, ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, const std::string & topic_name, @@ -133,8 +132,7 @@ apply_qos_override( template inline -void -declare_qos_parameters( +void declare_qos_parameters( const ::rclcpp::QosOverridingOptions & options, ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, const std::string & topic_name, From 6440680a07326bfc5ef488de688d934684dee9be Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 20 Oct 2020 14:58:21 -0300 Subject: [PATCH 05/38] Use new rmw functions Signed-off-by: Ivan Santiago Paunovic --- .../include/rclcpp/detail/qos_parameters.hpp | 244 ++++-------------- .../include/rclcpp/qos_overriding_options.hpp | 21 +- rclcpp/src/rclcpp/qos_overriding_options.cpp | 31 +-- .../rclcpp/detail/test_qos_parameters.cpp | 4 +- 4 files changed, 72 insertions(+), 228 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index 10ff3c9857..644f4a8c72 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -25,6 +25,8 @@ #include "rcl_interfaces/msg/parameter_descriptor.hpp" +#include "rmw/qos_string_conversions.h" + #include "rclcpp/duration.hpp" #include "rclcpp/node_interfaces/node_parameters_interface.hpp" #include "rclcpp/qos_overriding_options.hpp" @@ -45,7 +47,7 @@ struct PublisherQosParametersTraits QosPolicyKind::Deadline, QosPolicyKind::Durability, QosPolicyKind::History, - QosPolicyKind::HistoryDepth, + QosPolicyKind::Depth, QosPolicyKind::Lifespan, QosPolicyKind::Liveliness, QosPolicyKind::LivelinessLeaseDuration, @@ -65,7 +67,7 @@ struct SubscriptionQosParametersTraits QosPolicyKind::Deadline, QosPolicyKind::Durability, QosPolicyKind::History, - QosPolicyKind::HistoryDepth, + QosPolicyKind::Depth, QosPolicyKind::Liveliness, QosPolicyKind::LivelinessLeaseDuration, QosPolicyKind::Reliability, @@ -179,30 +181,17 @@ void declare_qos_parameters( } } -/// \internal Get the `rmw_qos_*_policy_t` value from a given `str`, or raise a runtime_error. -template -RetT -string_to_policy(const std::string & str); - -template<> -inline -rmw_qos_durability_policy_t -string_to_policy(const std::string & str); - -template<> -inline -rmw_qos_liveliness_policy_t -string_to_policy(const std::string & str); - -template<> -inline -rmw_qos_history_policy_t -string_to_policy(const std::string & str); - -template<> -inline -rmw_qos_reliability_policy_t -string_to_policy(const std::string & str); +/// \internal Helper function to get a rmw qos policy value from a string. +#define RCLCPP_DETAIL_QOS_POLICY_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 @@ -217,51 +206,35 @@ apply_qos_override( qos.deadline(::rclcpp::Duration(value.get())); break; case QosPolicyKind::Durability: - qos.durability(string_to_policy(value.get())); + RCLCPP_DETAIL_QOS_POLICY_FROM_PARAMETER_STRING( + durability, DURABILITY, value, qos); break; case QosPolicyKind::History: - qos.history(string_to_policy(value.get())); + RCLCPP_DETAIL_QOS_POLICY_FROM_PARAMETER_STRING( + history, HISTORY, value, qos); break; - case QosPolicyKind::HistoryDepth: + case QosPolicyKind::Depth: qos.get_rmw_qos_profile().depth = static_cast(value.get()); break; case QosPolicyKind::Lifespan: qos.lifespan(::rclcpp::Duration(value.get())); break; case QosPolicyKind::Liveliness: - qos.liveliness(string_to_policy(value.get())); + RCLCPP_DETAIL_QOS_POLICY_FROM_PARAMETER_STRING( + liveliness, LIVELINESS, value, qos); break; case QosPolicyKind::LivelinessLeaseDuration: qos.liveliness_lease_duration(::rclcpp::Duration(value.get())); break; case QosPolicyKind::Reliability: - qos.reliability(string_to_policy(value.get())); + RCLCPP_DETAIL_QOS_POLICY_FROM_PARAMETER_STRING( + reliability, RELIABILITY, value, qos); break; default: throw std::runtime_error{"unknown QosPolicyKind"}; } } -/// Convert the given policy to the corresponding string representation. -inline -const char * -policy_to_cstring(rmw_qos_durability_policy_t durability); - -/// Convert the given policy to the corresponding string representation. -inline -const char * -policy_to_cstring(rmw_qos_history_policy_t history); - -/// Convert the given policy to the corresponding string representation. -inline -const char * -policy_to_cstring(rmw_qos_liveliness_policy_t liveliness); - -/// Convert the given policy to the corresponding string representation. -inline -const char * -policy_to_cstring(rmw_qos_reliability_policy_t reliability); - /// Convert `rmw_time_t` to `int64_t` that can be used as a parameter value. inline int64_t @@ -273,6 +246,19 @@ rmw_duration_to_int64_t(rmw_time_t rmw_duration) ).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 ", std::ios::ate}; + oss << kind << " qos policy value: {" << policy_value_stringified << "}"; + throw std::invalid_argument{oss.str()}; + } + return policy_value_stringified; +} + inline ::rclcpp::ParameterValue get_default_qos_param_value(rclcpp::QosPolicyKind qpk, const rclcpp::QoS & qos) @@ -285,162 +271,32 @@ get_default_qos_param_value(rclcpp::QosPolicyKind qpk, const rclcpp::QoS & qos) case QosPolicyKind::Deadline: return ParameterValue(rmw_duration_to_int64_t(rmw_qos.deadline)); case QosPolicyKind::Durability: - return ParameterValue(policy_to_cstring(rmw_qos.durability)); + return ParameterValue( + check_if_stringified_policy_is_null( + rmw_qos_durability_policy_to_str(rmw_qos.durability), qpk)); case QosPolicyKind::History: - return ParameterValue(policy_to_cstring(rmw_qos.history)); - case QosPolicyKind::HistoryDepth: + return ParameterValue( + check_if_stringified_policy_is_null( + rmw_qos_history_policy_to_str(rmw_qos.history), qpk)); + 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(policy_to_cstring(rmw_qos.liveliness)); + return ParameterValue( + check_if_stringified_policy_is_null( + rmw_qos_liveliness_policy_to_str(rmw_qos.liveliness), qpk)); case QosPolicyKind::LivelinessLeaseDuration: return ParameterValue(rmw_duration_to_int64_t(rmw_qos.liveliness_lease_duration)); case QosPolicyKind::Reliability: - return ParameterValue(policy_to_cstring(rmw_qos.reliability)); + return ParameterValue( + check_if_stringified_policy_is_null( + rmw_qos_reliability_policy_to_str(rmw_qos.reliability), qpk)); default: throw std::invalid_argument{"unknown qos policy kind"}; } } -// TODO(ivanpauno): All `policy_to_cstring()` and `string_to_policy()` functions should be -// a wrapper of a `rcl` implemented function. -inline -const char * -policy_to_cstring(rmw_qos_durability_policy_t durability) -{ - switch (durability) { - case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: - return "system_default"; - case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: - return "transient_local"; - case RMW_QOS_POLICY_DURABILITY_VOLATILE: - return "volatile"; - case RMW_QOS_POLICY_DURABILITY_UNKNOWN: // fallthrough - default: - throw std::invalid_argument{"unknown durability qos policy value"}; - } -} - -inline -const char * -policy_to_cstring(rmw_qos_history_policy_t history) -{ - switch (history) { - case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: - return "system_default"; - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - return "keep_last"; - case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - return "keep_all"; - case RMW_QOS_POLICY_HISTORY_UNKNOWN: // fallthrough - default: - throw std::invalid_argument{"unknown history qos policy value"}; - } -} - -inline -const char * -policy_to_cstring(rmw_qos_liveliness_policy_t liveliness) -{ - switch (liveliness) { - case RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT: - return "system_default"; - case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: - return "automatic"; - case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: - return "manual_by_topic"; - case RMW_QOS_POLICY_LIVELINESS_UNKNOWN: // fallthrough - default: - throw std::invalid_argument{"unknown liveliness qos policy value"}; - } -} - -inline -const char * -policy_to_cstring(rmw_qos_reliability_policy_t reliability) -{ - switch (reliability) { - case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: - return "system_default"; - case RMW_QOS_POLICY_RELIABILITY_RELIABLE: - return "reliable"; - case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: - return "best_effort"; - case RMW_QOS_POLICY_RELIABILITY_UNKNOWN: // fallthrough - default: - throw std::invalid_argument{"unknown reliability qos policy value"}; - } -} - -template<> -inline -rmw_qos_durability_policy_t -string_to_policy(const std::string & str) -{ - if ("system_default" == str) { - return RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT; - } - if ("transient_local" == str) { - return RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - } - if ("volatile" == str) { - return RMW_QOS_POLICY_DURABILITY_VOLATILE; - } - throw std::invalid_argument{"unknown durability qos policy string"}; -} - -template<> -inline -rmw_qos_history_policy_t -string_to_policy(const std::string & str) -{ - if ("system_default" == str) { - return RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT; - } - if ("keep_last" == str) { - return RMW_QOS_POLICY_HISTORY_KEEP_LAST; - } - if ("keep_all" == str) { - return RMW_QOS_POLICY_HISTORY_KEEP_ALL; - } - throw std::invalid_argument{"unknown history qos policy string"}; -} - -template<> -inline -rmw_qos_liveliness_policy_t -string_to_policy(const std::string & str) -{ - if ("system_default" == str) { - return RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; - } - if ("automatic" == str) { - return RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - } - if ("manual_by_topic" == str) { - return RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; - } - throw std::invalid_argument{"unknown liveliness qos policy string"}; -} - -template<> -inline -rmw_qos_reliability_policy_t -string_to_policy(const std::string & str) -{ - if ("system_default" == str) { - return RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT; - } - if ("reliable" == str) { - return RMW_QOS_POLICY_RELIABILITY_RELIABLE; - } - if ("best_effort" == str) { - return RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - } - throw std::invalid_argument{"unknown reliability qos policy string"}; -} - } // namespace detail } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/qos_overriding_options.hpp b/rclcpp/include/rclcpp/qos_overriding_options.hpp index 826965ea1b..35d36b4cc4 100644 --- a/rclcpp/include/rclcpp/qos_overriding_options.hpp +++ b/rclcpp/include/rclcpp/qos_overriding_options.hpp @@ -24,20 +24,23 @@ #include "rclcpp/qos.hpp" +#include "rmw/qos_policy_kind.h" + namespace rclcpp { enum class QosPolicyKind { - AvoidRosNamespaceConventions, - Deadline, - Durability, - History, - HistoryDepth, - Lifespan, - Liveliness, - LivelinessLeaseDuration, - Reliability, + 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, }; const char * diff --git a/rclcpp/src/rclcpp/qos_overriding_options.cpp b/rclcpp/src/rclcpp/qos_overriding_options.cpp index 3adf89cb48..b89d4afadb 100644 --- a/rclcpp/src/rclcpp/qos_overriding_options.cpp +++ b/rclcpp/src/rclcpp/qos_overriding_options.cpp @@ -20,35 +20,20 @@ #include #include +#include "rmw/qos_policy_kind.h" +#include "rmw/qos_string_conversions.h" + namespace rclcpp { -// TODO(ivanpauno): this should be wrapping an rcl function const char * qos_policy_kind_to_cstr(const QosPolicyKind & qpk) { - switch (qpk) { - case QosPolicyKind::AvoidRosNamespaceConventions: - return "avoid_ros_namespace_conventions"; - case QosPolicyKind::Deadline: - return "deadline"; - case QosPolicyKind::Durability: - return "durability"; - case QosPolicyKind::History: - return "history"; - case QosPolicyKind::HistoryDepth: - return "history_depth"; - case QosPolicyKind::Lifespan: - return "lifespan"; - case QosPolicyKind::Liveliness: - return "liveliness"; - case QosPolicyKind::LivelinessLeaseDuration: - return "liveliness_lease_duration"; - case QosPolicyKind::Reliability: - return "reliability"; - default: - throw std::invalid_argument{"unknown qos policy kind"}; + 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 & @@ -58,7 +43,7 @@ operator<<(std::ostream & oss, const QosPolicyKind & qpk) } #define RCLCPP_QOS_OVERRIDING_OPTIONS_DEFAULT_QOS_POLICIES (std::initializer_list \ - {QosPolicyKind::History, QosPolicyKind::HistoryDepth, QosPolicyKind::Reliability}) + {QosPolicyKind::History, QosPolicyKind::Depth, QosPolicyKind::Reliability}) QosOverridingOptions::QosOverridingOptions(bool declare_default_parameters, std::string id) : id{std::move(id)}, diff --git a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp index aa9f4b83ca..67d3cc888c 100644 --- a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp @@ -30,7 +30,7 @@ TEST(TestQosParameters, test_overriding_options) { EXPECT_THAT( options.policy_kinds, testing::ElementsAre( rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::HistoryDepth, + rclcpp::QosPolicyKind::Depth, rclcpp::QosPolicyKind::Reliability)); } } @@ -57,7 +57,7 @@ TEST(TestQosParameters, declare) { "keep_last"); EXPECT_EQ( node->get_parameter( - "qos_overrides./my/fully/qualified/topic_name.publisher.history_depth").get_value(), + "qos_overrides./my/fully/qualified/topic_name.publisher.depth").get_value(), 10); EXPECT_EQ( node->get_parameter( From 6d21d39950f26ce7428a02ec3018a9f035fcae5d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 20 Oct 2020 16:08:48 -0300 Subject: [PATCH 06/38] Simplify code Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_publisher.hpp | 58 +---------- rclcpp/include/rclcpp/create_subscription.hpp | 46 +-------- .../include/rclcpp/detail/qos_parameters.hpp | 97 +++++++++---------- .../rclcpp/detail/test_qos_parameters.cpp | 7 +- 4 files changed, 56 insertions(+), 152 deletions(-) diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index 92b6539e08..99a0e018a3 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -19,7 +19,6 @@ #include #include -#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" #include "rclcpp/node_interfaces/node_topics_interface.hpp" #include "rclcpp/node_options.hpp" @@ -34,54 +33,6 @@ namespace rclcpp { -namespace detail -{ -// This is needed because it's currently possible to call `create_publisher()` -// only with a node topics interface. -// In that case, passing qos overridding options doesn't make sense. -// -// List of places in rclcpp passing directly a topic interface: -// creation of "/parameters_event" topic in node parameters interface (this one is tricky). -// creation of "topic statistics" in create_subscription -// -// TODO(ivanpauno): Write overload in which you can pass -// a topic interface and a parameter interface directly. -// TODO(ivanpauno): If we want the qos of `/parameters_event` topic to be reconfigurable, -// we need to figure out something. -template -std::enable_if_t::value, rclcpp::QoS> -get_publisher_actual_qos( - const rclcpp::QosOverridingOptions & options, NodeT & node, - std::string topic_name, rclcpp::QoS actual_qos) -{ - using rclcpp::node_interfaces::get_node_parameters_interface; - if (options.policy_kinds.size()) { - // TODO(ivanpauno) - // Get expanded and remapped topic name before creating the node. - // Need to refactor things in `rcl`. - detail::declare_publisher_qos_parameters( - options, - *get_node_parameters_interface(node), - topic_name, // this should be the expanded and remapped topic name - actual_qos); - } - return actual_qos; -} - -template -std::enable_if_t::value, rclcpp::QoS> -get_publisher_actual_qos( - const rclcpp::QosOverridingOptions & options, NodeT, std::string, rclcpp::QoS actual_qos) -{ - if (options.policy_kinds.size()) { - RCLCPP_WARN( - rclcpp::get_logger("rclcpp"), - "qos override options ignored because no parameter interface was provided"); - } - return actual_qos; -} -} // 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() @@ -103,12 +54,11 @@ 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 = rclcpp::node_interfaces::get_node_topics_interface(node); - using rclcpp::node_interfaces::get_node_parameters_interface; - rclcpp::QoS actual_qos = detail::get_publisher_actual_qos( - options.qos_overriding_options, node, topic_name, qos); + rclcpp::QoS actual_qos = rclcpp::detail::declare_qos_parameters( + options.qos_overriding_options, node, topic_name, + qos, rclcpp::detail::PublisherQosParametersTraits{}); // Create the publisher. auto pub = node_topics->create_publisher( diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 7b83105952..f285e09f05 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -41,47 +41,6 @@ namespace rclcpp { -namespace detail -{ -// See comment in `get_subscription_actual_qos`. -// -// List of places in rclcpp passing directly a topic interface: -// creation of "/clock" topic in time source -// (can be fixed by adding an overload accepting a parameters interface). -template -std::enable_if_t::value, rclcpp::QoS> -get_subscription_actual_qos( - const rclcpp::QosOverridingOptions & options, NodeT & node, - std::string topic_name, rclcpp::QoS actual_qos) -{ - using rclcpp::node_interfaces::get_node_parameters_interface; - if (options.policy_kinds.size()) { - // TODO(ivanpauno) - // Get expanded and remapped topic name before creating the node. - // Need to refactor things in `rcl`. - detail::declare_subscription_qos_parameters( - options, - *get_node_parameters_interface(node), - topic_name, // this should be the expanded and remapped topic name - actual_qos); - } - return actual_qos; -} - -template -std::enable_if_t::value, rclcpp::QoS> -get_subscription_actual_qos( - const rclcpp::QosOverridingOptions & options, NodeT, std::string, rclcpp::QoS actual_qos) -{ - if (options.policy_kinds.size()) { - RCLCPP_WARN( - rclcpp::get_logger("rclcpp"), - "qos override options ignored because no parameter interface was provided"); - } - return actual_qos; -} -} // 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() @@ -189,8 +148,9 @@ create_subscription( subscription_topic_stats ); - rclcpp::QoS actual_qos = detail::get_subscription_actual_qos( - options.qos_overriding_options, node, topic_name, qos); + rclcpp::QoS actual_qos = rclcpp::detail::declare_qos_parameters( + options.qos_overriding_options, node, topic_name, + qos, rclcpp::detail::SubscriptionQosParametersTraits{}); auto sub = node_topics->create_subscription(topic_name, factory, actual_qos); node_topics->add_subscription(sub, options.callback_group); diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index 644f4a8c72..b0f70b2361 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -24,10 +24,11 @@ #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" @@ -75,52 +76,6 @@ struct SubscriptionQosParametersTraits } }; -/// \internal Declare qos parameters for the given entity. -/** - * \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 parameters_interface Parameters will be declared through this interface. - * \param topic_name Name of the topic of the entity. - * \param qos User provided qos. It will be used as a default for the parameters declared, - * and then overriden with the final parameter overrides. - */ -template -inline -void declare_qos_parameters( - const ::rclcpp::QosOverridingOptions & options, - ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, - const std::string & topic_name, - ::rclcpp::QoS & qos, - EntityQosParametersTraits); - -/// \internal Same as `declare_qos_parameters()` for a `Publisher`. -inline -void -declare_publisher_qos_parameters( - const ::rclcpp::QosOverridingOptions & options, - ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, - const std::string & topic_name, - ::rclcpp::QoS & qos) -{ - declare_qos_parameters( - options, parameters_interface, topic_name, qos, PublisherQosParametersTraits{}); -} - -/// \internal Same as `declare_qos_parameters()` for a `Subscription`. -inline -void -declare_subscription_qos_parameters( - const ::rclcpp::QosOverridingOptions & options, - ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, - const std::string & topic_name, - ::rclcpp::QoS & qos) -{ - declare_qos_parameters( - options, parameters_interface, topic_name, qos, SubscriptionQosParametersTraits{}); -} - /// \internal Returns the given `policy` of the profile `qos` converted to a parameter value. inline ::rclcpp::ParameterValue @@ -132,15 +87,31 @@ void apply_qos_override( rclcpp::QosPolicyKind policy, rclcpp::ParameterValue value, rclcpp::QoS & qos); -template -inline -void declare_qos_parameters( +/// \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, + rclcpp::QoS> +declare_qos_parameters( const ::rclcpp::QosOverridingOptions & options, - ::rclcpp::node_interfaces::NodeParametersInterface & parameters_interface, + NodeT & node, const std::string & topic_name, - ::rclcpp::QoS & qos, + const ::rclcpp::QoS & default_qos, EntityQosParametersTraits) { + auto & parameters_interface = *rclcpp::node_interfaces::get_node_parameters_interface(node); std::string param_prefix; { std::ostringstream oss{"qos_overrides.", std::ios::ate}; @@ -160,6 +131,7 @@ void declare_qos_parameters( } param_description_suffix = oss.str(); } + rclcpp::QoS qos = default_qos; for (auto policy : EntityQosParametersTraits::allowed_policies()) { if ( std::count(options.policy_kinds.begin(), options.policy_kinds.end(), policy)) @@ -179,6 +151,27 @@ void declare_qos_parameters( if (options.validation_callback && !options.validation_callback(qos)) { throw rclcpp::exceptions::InvalidQosOverridesException{"validation callback failed"}; } + return qos; +} + +template +std::enable_if_t< + !rclcpp::node_interfaces::has_node_parameters_interface< + decltype(std::declval::type>())>::value, + rclcpp::QoS> +declare_qos_parameters( + const ::rclcpp::QosOverridingOptions & options, + NodeT &, + const std::string &, + const ::rclcpp::QoS & default_qos, + EntityQosParametersTraits) +{ + if (options.policy_kinds.size()) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "qos override options ignored because no parameter interface was provided"); + } + return default_qos; } /// \internal Helper function to get a rmw qos policy value from a string. diff --git a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp index 67d3cc888c..1a95055357 100644 --- a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp @@ -45,11 +45,12 @@ TEST(TestQosParameters, declare) { })); rclcpp::QoS qos{rclcpp::KeepLast(10)}; - rclcpp::detail::declare_publisher_qos_parameters( + qos = rclcpp::detail::declare_qos_parameters( rclcpp::QosOverridingOptions{true}, - *node->get_node_parameters_interface(), + node, "/my/fully/qualified/topic_name", - qos); + qos, + rclcpp::detail::PublisherQosParametersTraits{}); EXPECT_EQ( node->get_parameter( From ffd4659a439a836a32c9a940f11dbe1709cfec59 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 20 Oct 2020 18:44:23 -0300 Subject: [PATCH 07/38] Add overloads to create_publisher/create_subscription taking both a parameter_interface and a topics_interface Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_publisher.hpp | 68 ++++++-- rclcpp/include/rclcpp/create_subscription.hpp | 161 +++++++++++++----- 2 files changed, 172 insertions(+), 57 deletions(-) diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index 99a0e018a3..7043e4b128 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -33,19 +33,19 @@ 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 = ( @@ -53,25 +53,69 @@ create_publisher( ) ) { - // Extract the NodeTopicsInterface from the NodeT. - auto node_topics = rclcpp::node_interfaces::get_node_topics_interface(node); - + auto node_topics_interface = rclcpp::node_interfaces::get_node_topics_interface(node_topics); rclcpp::QoS actual_qos = rclcpp::detail::declare_qos_parameters( - options.qos_overriding_options, node, topic_name, + options.qos_overriding_options, node_parameters, topic_name, qos, rclcpp::detail::PublisherQosParametersTraits{}); // Create the publisher. - auto pub = node_topics->create_publisher( + auto pub = node_topics_interface->create_publisher( topic_name, rclcpp::create_publisher_factory(options), 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 & node_parameters, + rclcpp::node_interfaces::NodeParametersInterface & 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 f285e09f05..0c56af7281 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(std::forward(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( @@ -108,14 +85,15 @@ create_subscription( } std::shared_ptr> publisher = - create_publisher( - node, - options.topic_stats_options.publish_topic, - qos); // why the topics statistics publisher is using the same QoS as the subscription?? + rclcpp::detail::create_publisher( + node_parameters, + node_topics_interface, + options.topic_stats_options.publish_topic, + qos); // why the topics statistics publisher is using the same QoS as the subscription?? 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 ); @@ -149,14 +127,107 @@ create_subscription( ); rclcpp::QoS actual_qos = rclcpp::detail::declare_qos_parameters( - options.qos_overriding_options, node, topic_name, + options.qos_overriding_options, node_parameters, topic_name, qos, rclcpp::detail::SubscriptionQosParametersTraits{}); - auto sub = node_topics->create_subscription(topic_name, factory, actual_qos); - node_topics->add_subscription(sub, options.callback_group); + 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>( + std::forward(node), std::forward(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 && node_parameters, + rclcpp::node_interfaces::NodeTopicsInterface && 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>( + std::forward(node_parameters), + std::forward(node_topics), + topic_name, qos, std::forward(callback), options, msg_mem_strat); +} } // namespace rclcpp From 95068b58b391aa44399c944c3c8b9f99b1758f83 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 21 Oct 2020 11:18:16 -0300 Subject: [PATCH 08/38] Make use of qos overrides in some default topics Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_subscription.hpp | 4 +++- rclcpp/include/rclcpp/detail/qos_parameters.hpp | 2 ++ rclcpp/include/rclcpp/qos_overriding_options.hpp | 2 +- rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp | 1 + rclcpp/src/rclcpp/time_source.cpp | 9 ++++++++- 5 files changed, 15 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 0c56af7281..87b5790e04 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -84,12 +84,14 @@ create_subscription( " ms"); } + // TODO(ivanpauno): This could have topics statistics enabled, but I'm not sure if it makes + // sense. std::shared_ptr> publisher = rclcpp::detail::create_publisher( node_parameters, node_topics_interface, options.topic_stats_options.publish_topic, - qos); // why the topics statistics publisher is using the same QoS as the subscription?? + qos); subscription_topic_stats = std::make_shared< rclcpp::topic_statistics::SubscriptionTopicStatistics diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index b0f70b2361..d4583cdaf7 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -154,6 +154,8 @@ declare_qos_parameters( 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< diff --git a/rclcpp/include/rclcpp/qos_overriding_options.hpp b/rclcpp/include/rclcpp/qos_overriding_options.hpp index 35d36b4cc4..6508a272dd 100644 --- a/rclcpp/include/rclcpp/qos_overriding_options.hpp +++ b/rclcpp/include/rclcpp/qos_overriding_options.hpp @@ -91,8 +91,8 @@ struct QosOverridingOptions * \param policy_kinds list of policy kinds that will be reconfigurable. * \param id id of the entity. */ - QosOverridingOptions(std::initializer_list policy_kinds, std::string id = {}); + /// Construct passing a list of qos policies that and a verification callback. /** * This constructor is implicit, e.g.: 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/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 1beb54a61d..6c257fc428 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -27,6 +27,7 @@ #include "rclcpp/node.hpp" #include "rclcpp/parameter_client.hpp" #include "rclcpp/parameter_events_filter.hpp" +#include "rclcpp/subscription_options.hpp" #include "rclcpp/time.hpp" #include "rclcpp/time_source.hpp" @@ -233,11 +234,17 @@ void TimeSource::create_clock_sub() return; } + using rclcpp::QosPolicyKind; + rclcpp::SubscriptionOptions options; + options.qos_overriding_options = QosOverridingOptions{ + QosPolicyKind::Depth, QosPolicyKind::History, QosPolicyKind::LivelinessLeaseDuration, + QosPolicyKind::Reliability}; clock_subscription_ = rclcpp::create_subscription( node_topics_, "/clock", rclcpp::QoS(KeepLast(1)).best_effort(), - std::bind(&TimeSource::clock_cb, this, std::placeholders::_1) + std::bind(&TimeSource::clock_cb, this, std::placeholders::_1), + options ); } From 730c7419d0a47af2323d1fb98d7f5fab84e76929 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 21 Oct 2020 12:05:30 -0300 Subject: [PATCH 09/38] Please linters, add more notes Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_subscription.hpp | 24 +++++++++---------- .../src/rclcpp/node_interfaces/node_base.cpp | 1 + 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 87b5790e04..3115423c72 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -86,12 +86,12 @@ create_subscription( // TODO(ivanpauno): This could have topics statistics enabled, but I'm not sure if it makes // sense. - std::shared_ptr> publisher = - rclcpp::detail::create_publisher( - node_parameters, - node_topics_interface, - options.topic_stats_options.publish_topic, - qos); + 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 @@ -137,7 +137,7 @@ create_subscription( return std::dynamic_pointer_cast(sub); } -} // namespace detail +} // namespace detail /// Create and return a subscription of the given MessageT type. /** @@ -190,8 +190,8 @@ create_subscription( { return rclcpp::detail::create_subscription< MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT, MessageMemoryStrategyT>( - std::forward(node), std::forward(node), topic_name, qos, - std::forward(callback), options, msg_mem_strat); + std::forward(node), std::forward(node), topic_name, qos, + std::forward(callback), options, msg_mem_strat); } /// Create and return a subscription of the given MessageT type. @@ -226,9 +226,9 @@ create_subscription( { return rclcpp::detail::create_subscription< MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT, MessageMemoryStrategyT>( - std::forward(node_parameters), - std::forward(node_topics), - topic_name, qos, std::forward(callback), options, msg_mem_strat); + std::forward(node_parameters), + std::forward(node_topics), + topic_name, qos, std::forward(callback), options, msg_mem_strat); } } // namespace rclcpp 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. From 875c721e330e88f04cd7f4a6734b8008902f0ba8 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 21 Oct 2020 16:46:09 -0300 Subject: [PATCH 10/38] use resolved topic name Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_publisher.hpp | 9 ++++++--- rclcpp/include/rclcpp/create_subscription.hpp | 9 ++++++--- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index 7043e4b128..9b49611811 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -54,9 +54,12 @@ create_publisher( ) { auto node_topics_interface = rclcpp::node_interfaces::get_node_topics_interface(node_topics); - rclcpp::QoS actual_qos = rclcpp::detail::declare_qos_parameters( - options.qos_overriding_options, node_parameters, topic_name, - qos, rclcpp::detail::PublisherQosParametersTraits{}); + const rclcpp::QoS & actual_qos = options.qos_overriding_options.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_interface->create_publisher( diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 3115423c72..4cf6dc0cf1 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -128,9 +128,12 @@ create_subscription( subscription_topic_stats ); - rclcpp::QoS actual_qos = rclcpp::detail::declare_qos_parameters( - options.qos_overriding_options, node_parameters, topic_name, - qos, rclcpp::detail::SubscriptionQosParametersTraits{}); + const rclcpp::QoS & actual_qos = options.qos_overriding_options.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); From aafadb3ce9eae402958bf75c193000d5650baaa0 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 22 Oct 2020 13:49:14 -0300 Subject: [PATCH 11/38] Modify macro name Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/detail/qos_parameters.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index d4583cdaf7..3d0893b243 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -177,7 +177,7 @@ declare_qos_parameters( } /// \internal Helper function to get a rmw qos policy value from a string. -#define RCLCPP_DETAIL_QOS_POLICY_FROM_PARAMETER_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(); \ @@ -201,11 +201,11 @@ apply_qos_override( qos.deadline(::rclcpp::Duration(value.get())); break; case QosPolicyKind::Durability: - RCLCPP_DETAIL_QOS_POLICY_FROM_PARAMETER_STRING( + RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( durability, DURABILITY, value, qos); break; case QosPolicyKind::History: - RCLCPP_DETAIL_QOS_POLICY_FROM_PARAMETER_STRING( + RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( history, HISTORY, value, qos); break; case QosPolicyKind::Depth: @@ -215,14 +215,14 @@ apply_qos_override( qos.lifespan(::rclcpp::Duration(value.get())); break; case QosPolicyKind::Liveliness: - RCLCPP_DETAIL_QOS_POLICY_FROM_PARAMETER_STRING( + RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( liveliness, LIVELINESS, value, qos); break; case QosPolicyKind::LivelinessLeaseDuration: qos.liveliness_lease_duration(::rclcpp::Duration(value.get())); break; case QosPolicyKind::Reliability: - RCLCPP_DETAIL_QOS_POLICY_FROM_PARAMETER_STRING( + RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( reliability, RELIABILITY, value, qos); break; default: From 0cb162a618a17e56c440ff8c1f5548cd91523371 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 22 Oct 2020 13:49:38 -0300 Subject: [PATCH 12/38] Rename qpk to policy Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/detail/qos_parameters.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index 3d0893b243..ffa7d2d1f7 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -256,11 +256,11 @@ check_if_stringified_policy_is_null(const char * policy_value_stringified, QosPo inline ::rclcpp::ParameterValue -get_default_qos_param_value(rclcpp::QosPolicyKind qpk, const rclcpp::QoS & qos) +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 (qpk) { + switch (kind) { case QosPolicyKind::AvoidRosNamespaceConventions: return ParameterValue(rmw_qos.avoid_ros_namespace_conventions); case QosPolicyKind::Deadline: @@ -268,11 +268,11 @@ get_default_qos_param_value(rclcpp::QosPolicyKind qpk, const rclcpp::QoS & qos) case QosPolicyKind::Durability: return ParameterValue( check_if_stringified_policy_is_null( - rmw_qos_durability_policy_to_str(rmw_qos.durability), qpk)); + 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), qpk)); + rmw_qos_history_policy_to_str(rmw_qos.history), kind)); case QosPolicyKind::Depth: return ParameterValue(static_cast(rmw_qos.depth)); case QosPolicyKind::Lifespan: @@ -280,13 +280,13 @@ get_default_qos_param_value(rclcpp::QosPolicyKind qpk, const rclcpp::QoS & qos) case QosPolicyKind::Liveliness: return ParameterValue( check_if_stringified_policy_is_null( - rmw_qos_liveliness_policy_to_str(rmw_qos.liveliness), qpk)); + 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), qpk)); + rmw_qos_reliability_policy_to_str(rmw_qos.reliability), kind)); default: throw std::invalid_argument{"unknown qos policy kind"}; } From fa109249c672881d6e50e3f03ed0da0990fdf277 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 22 Oct 2020 13:53:06 -0300 Subject: [PATCH 13/38] reorder headers Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/node.hpp | 24 +++++++++++------------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index f4ea66e8be..c0e199298b 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -40,10 +40,21 @@ #include "rclcpp/client.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/context.hpp" +#include "rclcpp/detail/qos_parameters.hpp" #include "rclcpp/event.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/message_memory_strategy.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/node_interfaces/node_graph_interface.hpp" +#include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_time_source_interface.hpp" +#include "rclcpp/node_interfaces/node_timers_interface.hpp" +#include "rclcpp/node_interfaces/node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_waitables_interface.hpp" #include "rclcpp/node_options.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/publisher.hpp" @@ -58,19 +69,6 @@ #include "rclcpp/timer.hpp" #include "rclcpp/visibility_control.hpp" -#include "rclcpp/detail/qos_parameters.hpp" - -#include "rclcpp/node_interfaces/node_base_interface.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" -#include "rclcpp/node_interfaces/node_graph_interface.hpp" -#include "rclcpp/node_interfaces/node_logging_interface.hpp" -#include "rclcpp/node_interfaces/node_parameters_interface.hpp" -#include "rclcpp/node_interfaces/node_services_interface.hpp" -#include "rclcpp/node_interfaces/node_time_source_interface.hpp" -#include "rclcpp/node_interfaces/node_timers_interface.hpp" -#include "rclcpp/node_interfaces/node_topics_interface.hpp" -#include "rclcpp/node_interfaces/node_waitables_interface.hpp" - namespace rclcpp { From 58fcecfeda355d11af2ca12b87e948cc3e7d6ad4 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 22 Oct 2020 18:56:00 -0300 Subject: [PATCH 14/38] Fix bugs, add tests Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_publisher.hpp | 4 +-- rclcpp/include/rclcpp/create_subscription.hpp | 20 +++++------ .../include/rclcpp/detail/qos_parameters.hpp | 11 ++++-- rclcpp/src/rclcpp/time_source.cpp | 1 + .../rclcpp/detail/test_qos_parameters.cpp | 34 +++++++++++++++++++ 5 files changed, 54 insertions(+), 16 deletions(-) diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index 9b49611811..f864b43442 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -107,8 +107,8 @@ template< typename PublisherT = rclcpp::Publisher> std::shared_ptr create_publisher( - rclcpp::node_interfaces::NodeParametersInterface & node_parameters, - rclcpp::node_interfaces::NodeParametersInterface & node_topics, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_topics, const std::string & topic_name, const rclcpp::QoS & qos, const rclcpp::PublisherOptionsWithAllocator & options = ( diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 4cf6dc0cf1..13113a240d 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -54,8 +54,8 @@ template< typename NodeTopicsT> typename std::shared_ptr create_subscription( - NodeParametersT && node_parameters, - NodeTopicsT && node_topics, + NodeParametersT & node_parameters, + NodeTopicsT & node_topics, const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback, @@ -68,7 +68,7 @@ create_subscription( ) { using rclcpp::node_interfaces::get_node_topics_interface; - auto node_topics_interface = get_node_topics_interface(std::forward(node_topics)); + auto node_topics_interface = get_node_topics_interface(node_topics); std::shared_ptr> subscription_topic_stats = nullptr; @@ -179,7 +179,7 @@ template< typename NodeT> typename std::shared_ptr create_subscription( - NodeT && node, + NodeT & node, const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback, @@ -193,8 +193,7 @@ create_subscription( { return rclcpp::detail::create_subscription< MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT, MessageMemoryStrategyT>( - std::forward(node), std::forward(node), topic_name, qos, - std::forward(callback), options, msg_mem_strat); + node, node, topic_name, qos, std::forward(callback), options, msg_mem_strat); } /// Create and return a subscription of the given MessageT type. @@ -214,8 +213,8 @@ template< >> typename std::shared_ptr create_subscription( - rclcpp::node_interfaces::NodeParametersInterface && node_parameters, - rclcpp::node_interfaces::NodeTopicsInterface && node_topics, + 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, @@ -229,9 +228,8 @@ create_subscription( { return rclcpp::detail::create_subscription< MessageT, CallbackT, AllocatorT, CallbackMessageT, SubscriptionT, MessageMemoryStrategyT>( - std::forward(node_parameters), - std::forward(node_topics), - topic_name, qos, std::forward(callback), options, msg_mem_strat); + 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 index ffa7d2d1f7..d061f272c5 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include "rcl_interfaces/msg/parameter_descriptor.hpp" @@ -102,7 +103,9 @@ apply_qos_override( template std::enable_if_t< rclcpp::node_interfaces::has_node_parameters_interface< - decltype(std::declval::type>())>::value, + decltype(std::declval::type>())>::value || + std::is_same, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr>::value, rclcpp::QoS> declare_qos_parameters( const ::rclcpp::QosOverridingOptions & options, @@ -158,8 +161,10 @@ declare_qos_parameters( // was not provided. template std::enable_if_t< - !rclcpp::node_interfaces::has_node_parameters_interface< - decltype(std::declval::type>())>::value, + !(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, diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 6c257fc428..dc6bdff9df 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -240,6 +240,7 @@ void TimeSource::create_clock_sub() QosPolicyKind::Depth, QosPolicyKind::History, QosPolicyKind::LivelinessLeaseDuration, QosPolicyKind::Reliability}; clock_subscription_ = rclcpp::create_subscription( + node_parameters_, node_topics_, "/clock", rclcpp::QoS(KeepLast(1)).best_effort(), diff --git a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp index 1a95055357..a21acc0d18 100644 --- a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp @@ -77,3 +77,37 @@ TEST(TestQosParameters, declare) { rclcpp::shutdown(); } + +TEST(TestQosParameters, qos_parameters_created_by_one_node) { + rclcpp::init(0, nullptr); + auto node = std::make_shared( + "my_node", "/ns"); + + // Total number of qos parameters created by a node + // Up to now, /rosout and /parameter_events + // aren't creating qos parameters. + std::map qos_params; + EXPECT_FALSE( + node->get_node_parameters_interface()->get_parameters_by_prefix( + "qos_overrides", qos_params)); + + rclcpp::shutdown(); +} + +TEST(TestQosParameters, qos_parameters_created_by_one_node_with_use_sim_time) { + rclcpp::init(0, nullptr); + auto node = std::make_shared( + "my_node", "/ns", rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter("use_sim_time", true) + })); + + // Total number of qos parameters for the /clock topic is 4. + std::map qos_params; + EXPECT_TRUE( + node->get_node_parameters_interface()->get_parameters_by_prefix( + "qos_overrides", qos_params)); + EXPECT_EQ(4u, qos_params.size()); + + rclcpp::shutdown(); +} From 2424238669c6aed5305129d9ce213ef6dada51b3 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 23 Oct 2020 12:33:55 -0300 Subject: [PATCH 15/38] Tests for new create_publisher/create_subscription overloads Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_publisher.hpp | 2 +- rclcpp/test/rclcpp/test_create_subscription.cpp | 15 +++++++++++++++ rclcpp/test/rclcpp/test_publisher.cpp | 7 +++++++ 3 files changed, 23 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/create_publisher.hpp b/rclcpp/include/rclcpp/create_publisher.hpp index f864b43442..5f06122819 100644 --- a/rclcpp/include/rclcpp/create_publisher.hpp +++ b/rclcpp/include/rclcpp/create_publisher.hpp @@ -108,7 +108,7 @@ template< std::shared_ptr create_publisher( rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_parameters, - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_topics, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr & node_topics, const std::string & topic_name, const rclcpp::QoS & qos, const rclcpp::PublisherOptionsWithAllocator & options = ( diff --git a/rclcpp/test/rclcpp/test_create_subscription.cpp b/rclcpp/test/rclcpp/test_create_subscription.cpp index 184904b8e7..6502362680 100644 --- a/rclcpp/test/rclcpp/test_create_subscription.cpp +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -50,6 +50,21 @@ TEST_F(TestCreateSubscription, create) { 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..60ab066cc7 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -151,6 +151,13 @@ TEST_F(TestPublisher, various_creation_signatures) { rclcpp::create_publisher(node, "topic", 42, rclcpp::PublisherOptions()); (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; + } } /* From 5fdcb045905712a14b78719e929b12d1a6ad1797 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 23 Oct 2020 12:53:33 -0300 Subject: [PATCH 16/38] don't make /clock reconfigurable Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/time_source.cpp | 10 +---- .../rclcpp/detail/test_qos_parameters.cpp | 43 +++++++++++++++++++ 2 files changed, 44 insertions(+), 9 deletions(-) diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index dc6bdff9df..21ba5e8e99 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -234,19 +234,11 @@ void TimeSource::create_clock_sub() return; } - using rclcpp::QosPolicyKind; - rclcpp::SubscriptionOptions options; - options.qos_overriding_options = QosOverridingOptions{ - QosPolicyKind::Depth, QosPolicyKind::History, QosPolicyKind::LivelinessLeaseDuration, - QosPolicyKind::Reliability}; clock_subscription_ = rclcpp::create_subscription( - node_parameters_, node_topics_, "/clock", rclcpp::QoS(KeepLast(1)).best_effort(), - std::bind(&TimeSource::clock_cb, this, std::placeholders::_1), - options - ); + std::bind(&TimeSource::clock_cb, this, std::placeholders::_1)); } void TimeSource::destroy_clock_sub() diff --git a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp index a21acc0d18..d267807d9e 100644 --- a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp @@ -78,6 +78,49 @@ TEST(TestQosParameters, declare) { 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)}; +// qos = rclcpp::detail::declare_qos_parameters( +// rclcpp::QosOverridingOptions{[](const )}, +// 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, qos_parameters_created_by_one_node) { rclcpp::init(0, nullptr); auto node = std::make_shared( From 4d83c8bb445d2e00e630b839418a07191a8ce9cf Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 23 Oct 2020 14:01:49 -0300 Subject: [PATCH 17/38] Add tests for validation callback, cleanup QosOverridingOptions api Signed-off-by: Ivan Santiago Paunovic --- .../include/rclcpp/qos_overriding_options.hpp | 43 +-------- rclcpp/src/rclcpp/qos_overriding_options.cpp | 25 +----- .../rclcpp/detail/test_qos_parameters.cpp | 90 +++++-------------- 3 files changed, 27 insertions(+), 131 deletions(-) diff --git a/rclcpp/include/rclcpp/qos_overriding_options.hpp b/rclcpp/include/rclcpp/qos_overriding_options.hpp index 6508a272dd..dcffc72ea7 100644 --- a/rclcpp/include/rclcpp/qos_overriding_options.hpp +++ b/rclcpp/include/rclcpp/qos_overriding_options.hpp @@ -71,27 +71,8 @@ struct QosOverridingOptions /// Validation callback that will be called to verify the profile. QosCallback validation_callback; - /// Construct using default overriding options. - /** - * \param declare_default_parameters if `true`, the default set of qos that can be - * reconfigured will be declared. If `false`, qos aren't reconfigurable. - * \param id id of the entity. - */ - explicit QosOverridingOptions(bool declare_default_parameters = false, std::string id = {}); - - /// Construct passing a list of qos policies that can be overriden. - /** - * This constructor is implicit, e.g.: - * ```cpp - * node->create_publisher( - * "topic_name", - * default_qos_profile, - * {{QosPolicyKind::Reliability}, "my_id"}); - * ``` - * \param policy_kinds list of policy kinds that will be reconfigurable. - * \param id id of the entity. - */ - QosOverridingOptions(std::initializer_list policy_kinds, std::string id = {}); + /// Default constructor, no overrides allowed. + QosOverridingOptions() = default; /// Construct passing a list of qos policies that and a verification callback. /** @@ -113,26 +94,10 @@ struct QosOverridingOptions */ QosOverridingOptions( std::initializer_list policy_kinds, - QosCallback validation_callback, + QosCallback validation_callback = nullptr, std::string id = {}); - /// Construct using default overriding options and passing a validation callback. - /** - * This constructor is implicit, e.g.: - * ```cpp - * node->create_publisher( - * "topic_name", - * default_qos_profile, - * { - * [] (auto && qos) {return check_qos_validity(qos)}, - * "my_id" - * }); - * ``` - * \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. - */ - QosOverridingOptions(QosCallback validation_callback, std::string id = {}); // NOLINT, implicit + static std::initializer_list kDefaultPolicies; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/qos_overriding_options.cpp b/rclcpp/src/rclcpp/qos_overriding_options.cpp index b89d4afadb..ac3ec17d61 100644 --- a/rclcpp/src/rclcpp/qos_overriding_options.cpp +++ b/rclcpp/src/rclcpp/qos_overriding_options.cpp @@ -42,21 +42,8 @@ operator<<(std::ostream & oss, const QosPolicyKind & qpk) return oss << qos_policy_kind_to_cstr(qpk); } -#define RCLCPP_QOS_OVERRIDING_OPTIONS_DEFAULT_QOS_POLICIES (std::initializer_list \ - {QosPolicyKind::History, QosPolicyKind::Depth, QosPolicyKind::Reliability}) - -QosOverridingOptions::QosOverridingOptions(bool declare_default_parameters, std::string id) -: id{std::move(id)}, - policy_kinds{declare_default_parameters ? - RCLCPP_QOS_OVERRIDING_OPTIONS_DEFAULT_QOS_POLICIES : - std::initializer_list{}} -{} - -QosOverridingOptions::QosOverridingOptions( - std::initializer_list policy_kinds, std::string id) -: id{std::move(id)}, - policy_kinds{policy_kinds} -{} +std::initializer_list QosOverridingOptions::kDefaultPolicies = +{QosPolicyKind::History, QosPolicyKind::Depth, QosPolicyKind::Reliability}; QosOverridingOptions::QosOverridingOptions( std::initializer_list policy_kinds, @@ -67,12 +54,4 @@ QosOverridingOptions::QosOverridingOptions( validation_callback{std::move(validation_callback)} {} -QosOverridingOptions::QosOverridingOptions( - QosCallback validation_callback, - std::string id) -: id{std::move(id)}, - policy_kinds{RCLCPP_QOS_OVERRIDING_OPTIONS_DEFAULT_QOS_POLICIES}, - validation_callback{std::move(validation_callback)} -{} - } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp index d267807d9e..b0944faa4f 100644 --- a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp @@ -24,7 +24,7 @@ TEST(TestQosParameters, test_overriding_options) { { - rclcpp::QosOverridingOptions options{true}; + rclcpp::QosOverridingOptions options{rclcpp::QosOverridingOptions::kDefaultPolicies}; EXPECT_EQ(options.id, ""); EXPECT_EQ(options.validation_callback, nullptr); EXPECT_THAT( @@ -46,7 +46,7 @@ TEST(TestQosParameters, declare) { rclcpp::QoS qos{rclcpp::KeepLast(10)}; qos = rclcpp::detail::declare_qos_parameters( - rclcpp::QosOverridingOptions{true}, + rclcpp::QosOverridingOptions::kDefaultPolicies, node, "/my/fully/qualified/topic_name", qos, @@ -78,79 +78,31 @@ TEST(TestQosParameters, declare) { 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)}; -// qos = rclcpp::detail::declare_qos_parameters( -// rclcpp::QosOverridingOptions{[](const )}, -// 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, qos_parameters_created_by_one_node) { - rclcpp::init(0, nullptr); - auto node = std::make_shared( - "my_node", "/ns"); - - // Total number of qos parameters created by a node - // Up to now, /rosout and /parameter_events - // aren't creating qos parameters. - std::map qos_params; - EXPECT_FALSE( - node->get_node_parameters_interface()->get_parameters_by_prefix( - "qos_overrides", qos_params)); - - rclcpp::shutdown(); -} - -TEST(TestQosParameters, qos_parameters_created_by_one_node_with_use_sim_time) { +TEST(TestQosParameters, declare_with_callback) { rclcpp::init(0, nullptr); auto node = std::make_shared( "my_node", "/ns", rclcpp::NodeOptions().parameter_overrides( { - rclcpp::Parameter("use_sim_time", true) + rclcpp::Parameter( + "qos_overrides./my/fully/qualified/topic_name.publisher.reliability", "best_effort"), })); - // Total number of qos parameters for the /clock topic is 4. - std::map qos_params; - EXPECT_TRUE( - node->get_node_parameters_interface()->get_parameters_by_prefix( - "qos_overrides", qos_params)); - EXPECT_EQ(4u, qos_params.size()); + rclcpp::QoS qos{rclcpp::KeepLast(10)}; + EXPECT_THROW( + rclcpp::detail::declare_qos_parameters( + {rclcpp::QosOverridingOptions::kDefaultPolicies, [](const rclcpp::QoS &) {return false;}}, + node, + "/my/fully/qualified/topic_name/fails_validation", + qos, + rclcpp::detail::PublisherQosParametersTraits{}), + rclcpp::exceptions::InvalidQosOverridesException); + + rclcpp::detail::declare_qos_parameters( + {rclcpp::QosOverridingOptions::kDefaultPolicies, [](const rclcpp::QoS &) {return true;}}, + node, + "/my/fully/qualified/topic_name", + qos, + rclcpp::detail::PublisherQosParametersTraits{}); rclcpp::shutdown(); } From 3c5b855ab23e6b2f973466c156318cf8dce463a3 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 23 Oct 2020 15:41:54 -0300 Subject: [PATCH 18/38] Undo unneeded changes Signed-off-by: Ivan Santiago Paunovic --- rclcpp/src/rclcpp/time_source.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/time_source.cpp b/rclcpp/src/rclcpp/time_source.cpp index 21ba5e8e99..1beb54a61d 100644 --- a/rclcpp/src/rclcpp/time_source.cpp +++ b/rclcpp/src/rclcpp/time_source.cpp @@ -27,7 +27,6 @@ #include "rclcpp/node.hpp" #include "rclcpp/parameter_client.hpp" #include "rclcpp/parameter_events_filter.hpp" -#include "rclcpp/subscription_options.hpp" #include "rclcpp/time.hpp" #include "rclcpp/time_source.hpp" @@ -238,7 +237,8 @@ void TimeSource::create_clock_sub() node_topics_, "/clock", rclcpp::QoS(KeepLast(1)).best_effort(), - std::bind(&TimeSource::clock_cb, this, std::placeholders::_1)); + std::bind(&TimeSource::clock_cb, this, std::placeholders::_1) + ); } void TimeSource::destroy_clock_sub() From 5465266d0d70613c35938bb03f74311247fa159d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 30 Oct 2020 11:00:48 -0300 Subject: [PATCH 19/38] Remove comment Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/create_subscription.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 13113a240d..535bf0a50c 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -84,8 +84,6 @@ create_subscription( " ms"); } - // TODO(ivanpauno): This could have topics statistics enabled, but I'm not sure if it makes - // sense. std::shared_ptr> publisher = rclcpp::detail::create_publisher( node_parameters, From 5507be417a3d11fe41a25c421d6e02b003114d8f Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 30 Oct 2020 11:53:47 -0300 Subject: [PATCH 20/38] Write docs for QosOverridingOptions Signed-off-by: Ivan Santiago Paunovic --- .../include/rclcpp/qos_overriding_options.hpp | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/qos_overriding_options.hpp b/rclcpp/include/rclcpp/qos_overriding_options.hpp index dcffc72ea7..94c56ed0b1 100644 --- a/rclcpp/include/rclcpp/qos_overriding_options.hpp +++ b/rclcpp/include/rclcpp/qos_overriding_options.hpp @@ -60,7 +60,26 @@ class QosParameters; /// Options that are passed in subscription/publisher constructor to specify QoSConfigurability. /** - * TODO: Write nice docs here. + * 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 + * ``` */ struct QosOverridingOptions { From 091b6a21b067b40f384c39a94b37efd6b87a4181 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 30 Oct 2020 11:56:19 -0300 Subject: [PATCH 21/38] style Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Jacob Perron --- rclcpp/include/rclcpp/detail/qos_parameters.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index d061f272c5..6b4c71853f 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -88,7 +88,7 @@ void apply_qos_override( rclcpp::QosPolicyKind policy, rclcpp::ParameterValue value, rclcpp::QoS & qos); -/// \internal Declare qos parameters for the given entity. +/// \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 From 4121a6b626dbcf9c554c0468174f26b1b1b19310 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 30 Oct 2020 11:56:37 -0300 Subject: [PATCH 22/38] style Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Jacob Perron --- rclcpp/include/rclcpp/detail/qos_parameters.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index 6b4c71853f..78b2a37cb6 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -93,7 +93,7 @@ apply_qos_override( * \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 + * \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. From ea604e7fdecedc2a217fe7690865ca34a2e31ae8 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 30 Oct 2020 11:59:18 -0300 Subject: [PATCH 23/38] style Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Jacob Perron --- rclcpp/include/rclcpp/detail/qos_parameters.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index 78b2a37cb6..5386192e32 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -188,7 +188,7 @@ declare_qos_parameters( 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}; \ + throw std::invalid_argument{"unknown QoS policy " #kind_lower " value: " + policy_string}; \ } \ ((rclcpp_qos).kind_lower)(policy_value); \ } while (0) From aa5ff31aeab22181cd0f63406ee123e17f0a2576 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 30 Oct 2020 11:59:34 -0300 Subject: [PATCH 24/38] style Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Jacob Perron --- rclcpp/include/rclcpp/detail/qos_parameters.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index 5386192e32..caba644d31 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -253,7 +253,7 @@ check_if_stringified_policy_is_null(const char * policy_value_stringified, QosPo { if (!policy_value_stringified) { std::ostringstream oss{"unknown ", std::ios::ate}; - oss << kind << " qos policy value: {" << policy_value_stringified << "}"; + oss << kind << " QoS policy value: {" << policy_value_stringified << "}"; throw std::invalid_argument{oss.str()}; } return policy_value_stringified; From 1d168dd5a9d542c35ce930639e3ea558928a2747 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 30 Oct 2020 13:27:49 -0300 Subject: [PATCH 25/38] style Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Jacob Perron --- rclcpp/include/rclcpp/detail/qos_parameters.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index caba644d31..db1dcab04b 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -293,7 +293,7 @@ get_default_qos_param_value(rclcpp::QosPolicyKind kind, const rclcpp::QoS & qos) 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"}; + throw std::invalid_argument{"unknown QoS policy kind"}; } } From 927461a963ac6446c72527aaac14188a54f10262 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 30 Oct 2020 13:28:02 -0300 Subject: [PATCH 26/38] style Signed-off-by: Ivan Santiago Paunovic Co-authored-by: Jacob Perron --- rclcpp/src/rclcpp/qos_overriding_options.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/qos_overriding_options.cpp b/rclcpp/src/rclcpp/qos_overriding_options.cpp index ac3ec17d61..1325188de8 100644 --- a/rclcpp/src/rclcpp/qos_overriding_options.cpp +++ b/rclcpp/src/rclcpp/qos_overriding_options.cpp @@ -31,7 +31,7 @@ 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"}; + throw std::invalid_argument{"unknown QoS policy kind"}; } return ret; } From dd92127f212d2fa2f33f38e444299e2bf0dd48ca Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 5 Nov 2020 16:47:12 -0300 Subject: [PATCH 27/38] Use declare parameter or get Signed-off-by: Ivan Santiago Paunovic --- .../include/rclcpp/detail/qos_parameters.hpp | 21 +++++++++++++++++-- .../include/rclcpp/qos_overriding_options.hpp | 4 ++-- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index db1dcab04b..a7a3309d50 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -88,6 +88,22 @@ 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. @@ -146,8 +162,9 @@ declare_qos_parameters( rcl_interfaces::msg::ParameterDescriptor descriptor{}; descriptor.description = param_desciption.str(); descriptor.read_only = true; - auto value = parameters_interface.declare_parameter( - param_name.str(), get_default_qos_param_value(policy, qos), descriptor); + 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); } } diff --git a/rclcpp/include/rclcpp/qos_overriding_options.hpp b/rclcpp/include/rclcpp/qos_overriding_options.hpp index 94c56ed0b1..2c5cdc365d 100644 --- a/rclcpp/include/rclcpp/qos_overriding_options.hpp +++ b/rclcpp/include/rclcpp/qos_overriding_options.hpp @@ -65,9 +65,9 @@ class QosParameters; * - 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: From 96ea51a9723b2de279febb0363d943e480c8b189 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 5 Nov 2020 18:05:48 -0300 Subject: [PATCH 28/38] Improve tests Signed-off-by: Ivan Santiago Paunovic --- .../rclcpp/detail/test_qos_parameters.cpp | 60 ++++++++++--------- 1 file changed, 32 insertions(+), 28 deletions(-) diff --git a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp index b0944faa4f..fd19ef99e0 100644 --- a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp @@ -44,36 +44,40 @@ TEST(TestQosParameters, declare) { "qos_overrides./my/fully/qualified/topic_name.publisher.reliability", "best_effort"), })); - rclcpp::QoS qos{rclcpp::KeepLast(10)}; - qos = rclcpp::detail::declare_qos_parameters( - rclcpp::QosOverridingOptions::kDefaultPolicies, - node, - "/my/fully/qualified/topic_name", - qos, - rclcpp::detail::PublisherQosParametersTraits{}); + 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::kDefaultPolicies, + 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); + 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()); + 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(); } From a36a2195c895b467c8810129d61f5f7a27ffa24b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 5 Nov 2020 18:07:37 -0300 Subject: [PATCH 29/38] Remove rclcpp/test/detail folder Signed-off-by: Ivan Santiago Paunovic --- rclcpp/test/rclcpp/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/{detail => }/test_qos_parameters.cpp | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename rclcpp/test/rclcpp/{detail => }/test_qos_parameters.cpp (100%) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 0e7ca5724d..718b0d85ee 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -369,7 +369,7 @@ if(TARGET test_qos_event) mimick ) endif() -ament_add_gmock(test_qos_parameters detail/test_qos_parameters.cpp) +ament_add_gmock(test_qos_parameters test_qos_parameters.cpp) if(TARGET test_qos_parameters) target_link_libraries(test_qos_parameters ${PROJECT_NAME} diff --git a/rclcpp/test/rclcpp/detail/test_qos_parameters.cpp b/rclcpp/test/rclcpp/test_qos_parameters.cpp similarity index 100% rename from rclcpp/test/rclcpp/detail/test_qos_parameters.cpp rename to rclcpp/test/rclcpp/test_qos_parameters.cpp From e7355f756b50f32deafdda28d4da8c0913e3abb4 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 5 Nov 2020 18:21:42 -0300 Subject: [PATCH 30/38] Use Duration::from_nanoseconds Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/detail/qos_parameters.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index a7a3309d50..bec5b7b9e3 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -220,7 +220,7 @@ apply_qos_override( qos.avoid_ros_namespace_conventions(value.get()); break; case QosPolicyKind::Deadline: - qos.deadline(::rclcpp::Duration(value.get())); + qos.deadline(::rclcpp::Duration::from_nanoseconds(value.get())); break; case QosPolicyKind::Durability: RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( @@ -234,14 +234,14 @@ apply_qos_override( qos.get_rmw_qos_profile().depth = static_cast(value.get()); break; case QosPolicyKind::Lifespan: - qos.lifespan(::rclcpp::Duration(value.get())); + 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(value.get())); + qos.liveliness_lease_duration(::rclcpp::Duration::from_nanoseconds(value.get())); break; case QosPolicyKind::Reliability: RCLCPP_DETAIL_APPLY_QOS_OVERRIDE_FROM_PARAMETER_STRING( From 22a541269fb2e03a6c1b080ed0a7659b2a35f559 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 6 Nov 2020 18:18:40 -0300 Subject: [PATCH 31/38] Modify the qos validation callback signature so a reason of while the profile was rejected can be provided Signed-off-by: Ivan Santiago Paunovic --- .../include/rclcpp/detail/qos_parameters.hpp | 8 ++++++-- .../include/rclcpp/qos_overriding_options.hpp | 4 +++- rclcpp/test/rclcpp/test_qos_parameters.cpp | 18 ++++++++++++++++-- 3 files changed, 25 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index bec5b7b9e3..51554b6f6f 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -168,8 +168,12 @@ declare_qos_parameters( ::rclcpp::detail::apply_qos_override(policy, value, qos); } } - if (options.validation_callback && !options.validation_callback(qos)) { - throw rclcpp::exceptions::InvalidQosOverridesException{"validation callback failed"}; + if (options.validation_callback) { + auto result = options.validation_callback(qos); + if (!result.successful) { + throw rclcpp::exceptions::InvalidQosOverridesException{ + "validation callback failed: " + result.reason}; + } } return qos; } diff --git a/rclcpp/include/rclcpp/qos_overriding_options.hpp b/rclcpp/include/rclcpp/qos_overriding_options.hpp index 2c5cdc365d..3ee812c944 100644 --- a/rclcpp/include/rclcpp/qos_overriding_options.hpp +++ b/rclcpp/include/rclcpp/qos_overriding_options.hpp @@ -24,6 +24,7 @@ #include "rclcpp/qos.hpp" +#include "rcl_interfaces/msg/set_parameters_result.hpp" #include "rmw/qos_policy_kind.h" namespace rclcpp @@ -49,7 +50,8 @@ qos_policy_kind_to_cstr(const QosPolicyKind & qpk); std::ostream & operator<<(std::ostream & os, const QosPolicyKind & qpk); -using QosCallback = std::function; +using QosCallbackResult = rcl_interfaces::msg::SetParametersResult; +using QosCallback = std::function; namespace detail { diff --git a/rclcpp/test/rclcpp/test_qos_parameters.cpp b/rclcpp/test/rclcpp/test_qos_parameters.cpp index fd19ef99e0..ed41caf44b 100644 --- a/rclcpp/test/rclcpp/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/test_qos_parameters.cpp @@ -92,9 +92,15 @@ TEST(TestQosParameters, declare_with_callback) { })); rclcpp::QoS qos{rclcpp::KeepLast(10)}; + // *INDENT-OFF*, uncrustify suggestion makes the code unreadable EXPECT_THROW( rclcpp::detail::declare_qos_parameters( - {rclcpp::QosOverridingOptions::kDefaultPolicies, [](const rclcpp::QoS &) {return false;}}, + { + rclcpp::QosOverridingOptions::kDefaultPolicies, + [](const rclcpp::QoS &) { + return rclcpp::QosCallbackResult{}; + } + }, node, "/my/fully/qualified/topic_name/fails_validation", qos, @@ -102,11 +108,19 @@ TEST(TestQosParameters, declare_with_callback) { rclcpp::exceptions::InvalidQosOverridesException); rclcpp::detail::declare_qos_parameters( - {rclcpp::QosOverridingOptions::kDefaultPolicies, [](const rclcpp::QoS &) {return true;}}, + { + rclcpp::QosOverridingOptions::kDefaultPolicies, + [](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(); } From 71578d227d4d4930ff9246d90555e171c2b34644 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 13 Nov 2020 17:30:34 -0300 Subject: [PATCH 32/38] 100% line coverage in rclcpp/detail/qos_parameters.hpp Signed-off-by: Ivan Santiago Paunovic --- .../include/rclcpp/detail/qos_parameters.hpp | 8 +- rclcpp/test/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/test_qos_parameters.cpp | 148 +++++++++++++++++- 3 files changed, 152 insertions(+), 6 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/qos_parameters.hpp b/rclcpp/include/rclcpp/detail/qos_parameters.hpp index 51554b6f6f..9fa19a50e4 100644 --- a/rclcpp/include/rclcpp/detail/qos_parameters.hpp +++ b/rclcpp/include/rclcpp/detail/qos_parameters.hpp @@ -195,9 +195,9 @@ declare_qos_parameters( EntityQosParametersTraits) { if (options.policy_kinds.size()) { - RCLCPP_WARN( - rclcpp::get_logger("rclcpp"), - "qos override options ignored because no parameter interface was provided"); + std::runtime_error exc{ + "passed non-default qos overriding options without providing a parameters interface"}; + throw exc; } return default_qos; } @@ -252,7 +252,7 @@ apply_qos_override( reliability, RELIABILITY, value, qos); break; default: - throw std::runtime_error{"unknown QosPolicyKind"}; + throw std::invalid_argument{"unknown QosPolicyKind"}; } } diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 55b76faaf2..7023b8b905 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -4,7 +4,7 @@ find_package(test_msgs REQUIRED) include(cmake/rclcpp_add_build_failure_test.cmake) -add_subdirectory(benchmark) +# add_subdirectory(benchmark) add_subdirectory(rclcpp) ament_add_gtest(test_rclcpp_gtest_macros utils/test_rclcpp_gtest_macros.cpp) diff --git a/rclcpp/test/rclcpp/test_qos_parameters.cpp b/rclcpp/test/rclcpp/test_qos_parameters.cpp index ed41caf44b..2f1fb76ffa 100644 --- a/rclcpp/test/rclcpp/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/test_qos_parameters.cpp @@ -96,7 +96,7 @@ TEST(TestQosParameters, declare_with_callback) { EXPECT_THROW( rclcpp::detail::declare_qos_parameters( { - rclcpp::QosOverridingOptions::kDefaultPolicies, + {rclcpp::QosPolicyKind::Lifespan}, [](const rclcpp::QoS &) { return rclcpp::QosCallbackResult{}; } @@ -124,3 +124,149 @@ TEST(TestQosParameters, declare_with_callback) { 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::kDefaultPolicies, 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::kDefaultPolicies, + 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); +} From 17c136f722524f7f5513bd17c9beaaa095ac522f Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 13 Nov 2020 18:08:39 -0300 Subject: [PATCH 33/38] 100% coverage in create_publisher.hpp/create_subscription.hpp Signed-off-by: Ivan Santiago Paunovic --- rclcpp/test/rclcpp/test_create_subscription.cpp | 17 +++++++++++++++++ rclcpp/test/rclcpp/test_publisher.cpp | 8 ++++++++ 2 files changed, 25 insertions(+) diff --git a/rclcpp/test/rclcpp/test_create_subscription.cpp b/rclcpp/test/rclcpp/test_create_subscription.cpp index 6502362680..ad1baf5df3 100644 --- a/rclcpp/test/rclcpp/test_create_subscription.cpp +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -16,6 +16,7 @@ #include #include +#include #include "rclcpp/create_subscription.hpp" #include "rclcpp/node.hpp" @@ -50,6 +51,20 @@ 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{ + rclcpp::QosOverridingOptions::kDefaultPolicies}; + 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); @@ -60,6 +75,8 @@ TEST_F(TestCreateSubscription, create_separated_node_topics_and_parameters) { auto node_topics = node->get_node_topics_interface(); auto subscription = rclcpp::create_subscription( node_parameters, node_topics, "topic_name", qos, callback, options); + auto subscription2 = rclcpp::create_subscription( + node_parameters, node_topics, "topic_name", qos, std::move(callback), options); ASSERT_NE(nullptr, subscription); EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 60ab066cc7..0388210709 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -151,6 +151,14 @@ 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{ + rclcpp::QosOverridingOptions::kDefaultPolicies}; + 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(); From f65e971899b440409bc657f96e2db0423c0c8d47 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 13 Nov 2020 18:14:20 -0300 Subject: [PATCH 34/38] 100% coverage in test_qos_overriding_options.cpp Signed-off-by: Ivan Santiago Paunovic --- rclcpp/test/rclcpp/CMakeLists.txt | 6 ++++ .../rclcpp/test_qos_overriding_options.cpp | 36 +++++++++++++++++++ rclcpp/test/rclcpp/test_qos_parameters.cpp | 13 ------- 3 files changed, 42 insertions(+), 13 deletions(-) create mode 100644 rclcpp/test/rclcpp/test_qos_overriding_options.cpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 718b0d85ee..a1005836a0 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -369,6 +369,12 @@ 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 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..8c541d8329 --- /dev/null +++ b/rclcpp/test/rclcpp/test_qos_overriding_options.cpp @@ -0,0 +1,36 @@ +// 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) { + { + rclcpp::QosOverridingOptions options{rclcpp::QosOverridingOptions::kDefaultPolicies}; + EXPECT_EQ(options.id, ""); + EXPECT_EQ(options.validation_callback, nullptr); + EXPECT_THAT( + options.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 index 2f1fb76ffa..a60fcb8b99 100644 --- a/rclcpp/test/rclcpp/test_qos_parameters.cpp +++ b/rclcpp/test/rclcpp/test_qos_parameters.cpp @@ -22,19 +22,6 @@ #include "rclcpp/qos_overriding_options.hpp" #include "rclcpp/detail/qos_parameters.hpp" -TEST(TestQosParameters, test_overriding_options) { - { - rclcpp::QosOverridingOptions options{rclcpp::QosOverridingOptions::kDefaultPolicies}; - EXPECT_EQ(options.id, ""); - EXPECT_EQ(options.validation_callback, nullptr); - EXPECT_THAT( - options.policy_kinds, testing::ElementsAre( - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Reliability)); - } -} - TEST(TestQosParameters, declare) { rclcpp::init(0, nullptr); auto node = std::make_shared( From 3ee91e6b7f8b4d56dff9dcb65027d9e0028b7502 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 13 Nov 2020 18:15:13 -0300 Subject: [PATCH 35/38] Delete unnecessary includes Signed-off-by: Ivan Santiago Paunovic --- rclcpp/include/rclcpp/node.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index c0e199298b..f2247b59a1 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -40,7 +40,6 @@ #include "rclcpp/client.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/context.hpp" -#include "rclcpp/detail/qos_parameters.hpp" #include "rclcpp/event.hpp" #include "rclcpp/logger.hpp" #include "rclcpp/macros.hpp" @@ -60,7 +59,6 @@ #include "rclcpp/publisher.hpp" #include "rclcpp/publisher_options.hpp" #include "rclcpp/qos.hpp" -#include "rclcpp/qos_overriding_options.hpp" #include "rclcpp/service.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/subscription_options.hpp" From b9d71022ee36c29ecb0caf70c9856dd881e01e72 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 13 Nov 2020 18:28:02 -0300 Subject: [PATCH 36/38] Delete unnecesary test, please linters Signed-off-by: Ivan Santiago Paunovic --- .../test/rclcpp/test_create_subscription.cpp | 2 -- .../rclcpp/test_qos_overriding_options.cpp | 24 +++++++++---------- 2 files changed, 11 insertions(+), 15 deletions(-) diff --git a/rclcpp/test/rclcpp/test_create_subscription.cpp b/rclcpp/test/rclcpp/test_create_subscription.cpp index ad1baf5df3..dd6f3cc23a 100644 --- a/rclcpp/test/rclcpp/test_create_subscription.cpp +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -75,8 +75,6 @@ TEST_F(TestCreateSubscription, create_separated_node_topics_and_parameters) { auto node_topics = node->get_node_topics_interface(); auto subscription = rclcpp::create_subscription( node_parameters, node_topics, "topic_name", qos, callback, options); - auto subscription2 = rclcpp::create_subscription( - node_parameters, node_topics, "topic_name", qos, std::move(callback), options); ASSERT_NE(nullptr, subscription); EXPECT_STREQ("/ns/topic_name", subscription->get_topic_name()); diff --git a/rclcpp/test/rclcpp/test_qos_overriding_options.cpp b/rclcpp/test/rclcpp/test_qos_overriding_options.cpp index 8c541d8329..be3f276db7 100644 --- a/rclcpp/test/rclcpp/test_qos_overriding_options.cpp +++ b/rclcpp/test/rclcpp/test_qos_overriding_options.cpp @@ -17,20 +17,18 @@ #include "rclcpp/qos_overriding_options.hpp" TEST(TestQosOverridingOptions, test_overriding_options) { - { - rclcpp::QosOverridingOptions options{rclcpp::QosOverridingOptions::kDefaultPolicies}; - EXPECT_EQ(options.id, ""); - EXPECT_EQ(options.validation_callback, nullptr); - EXPECT_THAT( - options.policy_kinds, testing::ElementsAre( - rclcpp::QosPolicyKind::History, - rclcpp::QosPolicyKind::Depth, - rclcpp::QosPolicyKind::Reliability)); - } + rclcpp::QosOverridingOptions options{rclcpp::QosOverridingOptions::kDefaultPolicies}; + EXPECT_EQ(options.id, ""); + EXPECT_EQ(options.validation_callback, nullptr); + EXPECT_THAT( + options.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); + EXPECT_THROW( + rclcpp::qos_policy_kind_to_cstr(rclcpp::QosPolicyKind::Invalid), + std::invalid_argument); } From 04398a4915d404f2c8af412b9986320099938d83 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 16 Nov 2020 12:32:08 -0300 Subject: [PATCH 37/38] Uncomment line Signed-off-by: Ivan Santiago Paunovic --- rclcpp/test/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index 7023b8b905..55b76faaf2 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -4,7 +4,7 @@ find_package(test_msgs REQUIRED) include(cmake/rclcpp_add_build_failure_test.cmake) -# add_subdirectory(benchmark) +add_subdirectory(benchmark) add_subdirectory(rclcpp) ament_add_gtest(test_rclcpp_gtest_macros utils/test_rclcpp_gtest_macros.cpp) From da0742316caad9a3de6f33cbe3b17d911b6ccad0 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 16 Nov 2020 12:44:15 -0300 Subject: [PATCH 38/38] Delete unneeded include Signed-off-by: Ivan Santiago Paunovic --- rclcpp/test/rclcpp/test_create_subscription.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/test/rclcpp/test_create_subscription.cpp b/rclcpp/test/rclcpp/test_create_subscription.cpp index dd6f3cc23a..68ca768594 100644 --- a/rclcpp/test/rclcpp/test_create_subscription.cpp +++ b/rclcpp/test/rclcpp/test_create_subscription.cpp @@ -16,7 +16,6 @@ #include #include -#include #include "rclcpp/create_subscription.hpp" #include "rclcpp/node.hpp"