Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add check for invalid topic statistics publish period #1151

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 25 additions & 0 deletions rclcpp/include/rclcpp/create_subscription.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <chrono>
#include <functional>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>

Expand All @@ -44,6 +45,23 @@ namespace rclcpp
* 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,
Expand Down Expand Up @@ -81,6 +99,13 @@ create_subscription(
options,
*node_topics->get_node_base_interface()))
{
if (options.topic_stats_options.publish_period <= std::chrono::milliseconds(0)) {
throw std::invalid_argument(
"topic_stats_options.publish_period must be greater than 0, specified value of " +
std::to_string(options.topic_stats_options.publish_period.count()) +
" ms");
}

std::shared_ptr<Publisher<statistics_msgs::msg::MetricsMessage>> publisher =
create_publisher<statistics_msgs::msg::MetricsMessage>(
node,
Expand Down
3 changes: 2 additions & 1 deletion rclcpp/include/rclcpp/subscription_options.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,8 @@ struct SubscriptionOptionsBase
// Topic to which topic statistics get published when enabled. Defaults to /statistics.
std::string publish_topic = "/statistics";

// Topic statistics publication period in ms. Defaults to one minute.
// Topic statistics publication period in ms. Defaults to one second.
dabonnie marked this conversation as resolved.
Show resolved Hide resolved
// Only values greater than zero are allowed.
std::chrono::milliseconds publish_period{std::chrono::seconds(1)};
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <iostream>
#include <memory>
#include <set>
#include <stdexcept>
#include <string>
#include <vector>

Expand Down Expand Up @@ -223,6 +224,30 @@ class TestSubscriptionTopicStatisticsFixture : public ::testing::Test
std::shared_ptr<EmptySubscriber> empty_subscriber;
};

TEST(TestSubscriptionTopicStatistics, test_invalid_publish_period)
{
rclcpp::init(0 /* argc */, nullptr /* argv */);

auto node = std::make_shared<rclcpp::Node>("test_period_node");

auto options = rclcpp::SubscriptionOptions();
options.topic_stats_options.state = rclcpp::TopicStatisticsState::Enable;
options.topic_stats_options.publish_period = std::chrono::milliseconds(0);

auto callback = [](Empty::UniquePtr msg) {
(void) msg;
};

ASSERT_THROW(
(node->create_subscription<Empty, std::function<void(Empty::UniquePtr)>>(
"should_throw_invalid_arg",
rclcpp::QoS(rclcpp::KeepAll()),
callback,
options)), std::invalid_argument);

rclcpp::shutdown();
}

TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction)
{
// Manually create publisher tied to the node
Expand Down