Skip to content

Commit

Permalink
Add NodeDefault option for enabling topic statistics
Browse files Browse the repository at this point in the history
Signed-off-by: Prajakta Gokhale <prajaktg@amazon.com>
  • Loading branch information
Prajakta Gokhale committed Apr 21, 2020
1 parent 61e5075 commit a0bc80e
Show file tree
Hide file tree
Showing 11 changed files with 202 additions and 13 deletions.
54 changes: 54 additions & 0 deletions rclcpp/include/rclcpp/detail/resolve_enable_topic_statistics.hpp
Original file line number Diff line number Diff line change
@@ -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 <stdexcept>

#include "rclcpp/topic_statistics_state.hpp"

namespace rclcpp
{
namespace detail
{

/// Return whether or not topic statistics is enabled, resolving "NodeDefault" if needed.
template<typename OptionsT, typename NodeBaseT>
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_
6 changes: 6 additions & 0 deletions rclcpp/include/rclcpp/node_impl.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -91,6 +92,11 @@ Node::create_subscription(
const SubscriptionOptionsWithAllocator<AllocatorT> & options,
typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
{
// Setup topic statistics collector if enabled.
if (rclcpp::detail::resolve_enable_topic_statistics(options, *node_base_)) {
// TODO(@dabonnie, @prajakta-gokhale): https://github.com/ros2/ros2/issues/901.
}

return rclcpp::create_subscription<MessageT>(
*this,
extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
Expand Down
7 changes: 6 additions & 1 deletion rclcpp/include/rclcpp/node_interfaces/node_base.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,8 @@ class NodeBase : public NodeBaseInterface
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
Expand Down Expand Up @@ -133,11 +134,15 @@ class NodeBase : public NodeBaseInterface
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<rcl_node_t> node_handle_;

Expand Down
6 changes: 6 additions & 0 deletions rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,12 @@ class NodeBaseInterface
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
Expand Down
20 changes: 20 additions & 0 deletions rclcpp/include/rclcpp/node_options.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ class NodeOptions
* - 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
Expand Down Expand Up @@ -187,6 +188,23 @@ class NodeOptions
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
Expand Down Expand Up @@ -332,6 +350,8 @@ class NodeOptions

bool use_intra_process_comms_ {false};

bool enable_topic_statistics_ {false};

bool start_parameter_services_ {true};

bool start_parameter_event_publisher_ {true};
Expand Down
8 changes: 2 additions & 6 deletions rclcpp/include/rclcpp/subscription_options.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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";
Expand Down Expand Up @@ -123,8 +121,6 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase
};

using SubscriptionOptions = SubscriptionOptionsWithAllocator<std::allocator<void>>;
using TopicStatisticsState = SubscriptionOptionsBase::TopicStatisticsOptions::TopicStatisticsState;

} // namespace rclcpp

#endif // RCLCPP__SUBSCRIPTION_OPTIONS_HPP_
35 changes: 35 additions & 0 deletions rclcpp/include/rclcpp/topic_statistics_state.hpp
Original file line number Diff line number Diff line change
@@ -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_
3 changes: 2 additions & 1 deletion rclcpp/src/rclcpp/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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())),
Expand Down
10 changes: 9 additions & 1 deletion rclcpp/src/rclcpp/node_interfaces/node_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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_;
}
14 changes: 14 additions & 0 deletions rclcpp/src/rclcpp/node_options.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down Expand Up @@ -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
{
Expand Down
52 changes: 48 additions & 4 deletions rclcpp/test/test_subscription_options.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,9 +15,12 @@
#include <gtest/gtest.h>

#include <chrono>
#include <memory>
#include <string>
#include <vector>

#include "rclcpp/node.hpp"
#include "rclcpp/node_options.hpp"
#include "rclcpp/subscription_options.hpp"

using namespace std::chrono_literals;
Expand All @@ -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<rclcpp::Node>("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())));
}

0 comments on commit a0bc80e

Please sign in to comment.