diff --git a/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp b/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp new file mode 100644 index 0000000..7c59204 --- /dev/null +++ b/rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp @@ -0,0 +1,54 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// 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__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_ +#define RCLCPP__DETAIL__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_ + +#include + +#include "rclcpp/topic_statistics_state.hpp" + +namespace rclcpp +{ +namespace detail +{ + +/// Return whether or not topic statistics is enabled, resolving "NodeDefault" if needed. +template +bool +resolve_enable_topic_statistics(const OptionsT & options, const NodeBaseT & node_base) +{ + bool topic_stats_enabled; + switch (options.topic_stats_options.state) { + case TopicStatisticsState::Enable: + topic_stats_enabled = true; + break; + case TopicStatisticsState::Disable: + topic_stats_enabled = false; + break; + case TopicStatisticsState::NodeDefault: + topic_stats_enabled = node_base.get_enable_topic_statistics_default(); + break; + default: + throw std::runtime_error("Unrecognized EnableTopicStatistics value"); + break; + } + + return topic_stats_enabled; +} + +} // namespace detail +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__RESOLVE_ENABLE_TOPIC_STATISTICS_HPP_ diff --git a/rclcpp/include/rclcpp/node_impl.hpp b/rclcpp/include/rclcpp/node_impl.hpp index b6ed04b..e29edeb 100644 --- a/rclcpp/include/rclcpp/node_impl.hpp +++ b/rclcpp/include/rclcpp/node_impl.hpp @@ -38,6 +38,7 @@ #include "rclcpp/create_publisher.hpp" #include "rclcpp/create_service.hpp" #include "rclcpp/create_subscription.hpp" +#include "rclcpp/detail/resolve_enable_topic_statistics.hpp" #include "rclcpp/parameter.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index 38f8d75..c21514f 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -42,7 +42,8 @@ public: const std::string & namespace_, rclcpp::Context::SharedPtr context, const rcl_node_options_t & rcl_node_options, - bool use_intra_process_default); + bool use_intra_process_default, + bool enable_topic_statistics_default); RCLCPP_PUBLIC virtual @@ -133,11 +134,15 @@ public: bool get_use_intra_process_default() const override; + bool + get_enable_topic_statistics_default() const override; + private: RCLCPP_DISABLE_COPY(NodeBase) rclcpp::Context::SharedPtr context_; bool use_intra_process_default_; + bool enable_topic_statistics_default_; std::shared_ptr node_handle_; diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index d0247ee..eb7d34d 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -161,6 +161,12 @@ public: virtual bool get_use_intra_process_default() const = 0; + + /// Return the default preference for enabling topic statistics collection. + RCLCPP_PUBLIC + virtual + bool + get_enable_topic_statistics_default() const = 0; }; } // namespace node_interfaces diff --git a/rclcpp/include/rclcpp/node_options.hpp b/rclcpp/include/rclcpp/node_options.hpp index 57df850..c137895 100644 --- a/rclcpp/include/rclcpp/node_options.hpp +++ b/rclcpp/include/rclcpp/node_options.hpp @@ -43,6 +43,7 @@ public: * - parameter_overrides = {} * - use_global_arguments = true * - use_intra_process_comms = false + * - enable_topic_statistics = false * - start_parameter_services = true * - start_parameter_event_publisher = true * - parameter_event_qos = rclcpp::ParameterEventQoS @@ -187,6 +188,23 @@ public: NodeOptions & use_intra_process_comms(bool use_intra_process_comms); + /// Return the enable_topic_statistics flag. + RCLCPP_PUBLIC + bool + enable_topic_statistics() const; + + /// Set the enable_topic_statistics flag, return this for parameter idiom. + /** + * If true, topic statistics collection and publication will be enabled + * for all subscriptions. + * This can be used to override the global topic statistics setting. + * + * Defaults to false. + */ + RCLCPP_PUBLIC + NodeOptions & + enable_topic_statistics(bool enable_topic_statistics); + /// Return the start_parameter_services flag. RCLCPP_PUBLIC bool @@ -332,6 +350,8 @@ private: bool use_intra_process_comms_ {false}; + bool enable_topic_statistics_ {false}; + bool start_parameter_services_ {true}; bool start_parameter_event_publisher_ {true}; diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 72e1f4b..e631ba5 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/topic_statistics_state.hpp" #include "rclcpp/visibility_control.hpp" namespace rclcpp @@ -59,11 +60,8 @@ struct SubscriptionOptionsBase // Options to configure topic statistics collector in the subscription. struct TopicStatisticsOptions { - // Represent the state of topic statistics collector. - enum class TopicStatisticsState {ENABLED, DISABLED}; - // Enable and disable topic statistics calculation and publication. Defaults to disabled. - TopicStatisticsState state = TopicStatisticsState::DISABLED; + TopicStatisticsState state = TopicStatisticsState::NodeDefault; // Topic to which topic statistics get published when enabled. Defaults to /statistics. std::string publish_topic = "/statistics"; @@ -123,8 +121,6 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase }; using SubscriptionOptions = SubscriptionOptionsWithAllocator>; -using TopicStatisticsState = SubscriptionOptionsBase::TopicStatisticsOptions::TopicStatisticsState; - } // namespace rclcpp #endif // RCLCPP__SUBSCRIPTION_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/topic_statistics_state.hpp b/rclcpp/include/rclcpp/topic_statistics_state.hpp new file mode 100644 index 0000000..b407622 --- /dev/null +++ b/rclcpp/include/rclcpp/topic_statistics_state.hpp @@ -0,0 +1,35 @@ +// Copyright 2019 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__TOPIC_STATISTICS_STATE_HPP_ +#define RCLCPP__TOPIC_STATISTICS_STATE_HPP_ + +namespace rclcpp +{ + +/// Represent the state of topic statistics collector. +/// Used as argument in create_subscriber. +enum class TopicStatisticsState +{ + /// Explicitly enable topic statistics at subscription level. + Enable, + /// Explicitly disable topic statistics at subscription level. + Disable, + /// Take topic statistics state from the node. + NodeDefault +}; + +} // namespace rclcpp + +#endif // RCLCPP__TOPIC_STATISTICS_STATE_HPP_ diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index a2bf270..b429b30 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -103,7 +103,8 @@ Node::Node( namespace_, options.context(), *(options.get_rcl_node_options()), - options.use_intra_process_comms())), + options.use_intra_process_comms(), + options.enable_topic_statistics())), node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())), node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())), node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())), diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 9241adf..6a6f1e4 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -34,9 +34,11 @@ NodeBase::NodeBase( const std::string & namespace_, rclcpp::Context::SharedPtr context, const rcl_node_options_t & rcl_node_options, - bool use_intra_process_default) + bool use_intra_process_default, + bool enable_topic_statistics_default) : context_(context), use_intra_process_default_(use_intra_process_default), + enable_topic_statistics_default_(enable_topic_statistics_default), node_handle_(nullptr), default_callback_group_(nullptr), associated_with_executor_(false), @@ -268,3 +270,9 @@ NodeBase::get_use_intra_process_default() const { return use_intra_process_default_; } + +bool +NodeBase::get_enable_topic_statistics_default() const +{ + return enable_topic_statistics_default_; +} diff --git a/rclcpp/src/rclcpp/node_options.cpp b/rclcpp/src/rclcpp/node_options.cpp index af424ae..cac10e6 100644 --- a/rclcpp/src/rclcpp/node_options.cpp +++ b/rclcpp/src/rclcpp/node_options.cpp @@ -73,6 +73,7 @@ NodeOptions::operator=(const NodeOptions & other) this->use_global_arguments_ = other.use_global_arguments_; this->enable_rosout_ = other.enable_rosout_; this->use_intra_process_comms_ = other.use_intra_process_comms_; + this->enable_topic_statistics_ = other.enable_topic_statistics_; this->start_parameter_services_ = other.start_parameter_services_; this->allocator_ = other.allocator_; this->allow_undeclared_parameters_ = other.allow_undeclared_parameters_; @@ -213,6 +214,19 @@ NodeOptions::use_intra_process_comms(bool use_intra_process_comms) return *this; } +bool +NodeOptions::enable_topic_statistics() const +{ + return this->enable_topic_statistics_; +} + +NodeOptions & +NodeOptions::enable_topic_statistics(bool enable_topic_statistics) +{ + this->enable_topic_statistics_ = enable_topic_statistics; + return *this; +} + bool NodeOptions::start_parameter_services() const { diff --git a/rclcpp/test/test_subscription_options.cpp b/rclcpp/test/test_subscription_options.cpp index e884b03..ee24685 100644 --- a/rclcpp/test/test_subscription_options.cpp +++ b/rclcpp/test/test_subscription_options.cpp @@ -15,9 +15,12 @@ #include #include +#include #include #include +#include "rclcpp/node.hpp" +#include "rclcpp/node_options.hpp" #include "rclcpp/subscription_options.hpp" using namespace std::chrono_literals; @@ -27,18 +30,59 @@ namespace constexpr const char defaultPublishTopic[] = "/statistics"; } -TEST(TestSubscriptionOptions, topic_statistics_options) { +class TestSubscriptionOptions : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + +protected: + void initialize(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions()) + { + node = std::make_shared("test_subscription_options", node_options); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +TEST_F(TestSubscriptionOptions, topic_statistics_options_default_and_set) { auto options = rclcpp::SubscriptionOptions(); - EXPECT_EQ(options.topic_stats_options.state, rclcpp::TopicStatisticsState::DISABLED); + EXPECT_EQ(options.topic_stats_options.state, rclcpp::TopicStatisticsState::NodeDefault); EXPECT_EQ(options.topic_stats_options.publish_topic, defaultPublishTopic); EXPECT_EQ(options.topic_stats_options.publish_period, 1s); - options.topic_stats_options.state = rclcpp::TopicStatisticsState::ENABLED; + options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable; options.topic_stats_options.publish_topic = "topic_statistics"; options.topic_stats_options.publish_period = 5min; - EXPECT_EQ(options.topic_stats_options.state, rclcpp::TopicStatisticsState::ENABLED); + EXPECT_EQ(options.topic_stats_options.state, rclcpp::TopicStatisticsState::Enable); EXPECT_EQ(options.topic_stats_options.publish_topic, "topic_statistics"); EXPECT_EQ(options.topic_stats_options.publish_period, 5min); } + +TEST_F(TestSubscriptionOptions, topic_statistics_options_node_default_mode) { + initialize(); + auto subscription_options = rclcpp::SubscriptionOptions(); + + EXPECT_EQ( + subscription_options.topic_stats_options.state, + rclcpp::TopicStatisticsState::NodeDefault); + EXPECT_FALSE( + rclcpp::detail::resolve_enable_topic_statistics( + subscription_options, + *(node->get_node_base_interface()))); + + initialize(rclcpp::NodeOptions().enable_topic_statistics(true)); + EXPECT_TRUE( + rclcpp::detail::resolve_enable_topic_statistics( + subscription_options, + *(node->get_node_base_interface()))); +}