Skip to content
This repository has been archived by the owner on Jun 10, 2021. It is now read-only.

Use rclcpp::NodeOptions during node construction #73

Merged
merged 8 commits into from Jan 17, 2020
Merged
Show file tree
Hide file tree
Changes from 4 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
Expand Up @@ -15,7 +15,6 @@
#ifndef SYSTEM_METRICS_COLLECTOR__CONSTANTS_HPP_
#define SYSTEM_METRICS_COLLECTOR__CONSTANTS_HPP_

#include <chrono>

namespace system_metrics_collector
{
Expand All @@ -24,8 +23,12 @@ namespace collector_node_constants
{

constexpr const char kStatisticsTopicName[] = "system_metrics";
constexpr const std::chrono::seconds kDefaultCollectPeriod{1};
constexpr const std::chrono::minutes kDefaultPublishPeriod{1};

constexpr const char kCollectPeriodParam[] = "measurement_period";
constexpr const int64_t kDefaultCollectPeriodInMs = 1000; // 1 second

constexpr const char kPublishPeriodParam[] = "publish_period";
constexpr const int64_t kDefaultPublishPeriodInMs = 60000; // 1 minute

} // namespace collector_node_constants

Expand Down
Expand Up @@ -31,9 +31,7 @@ int main(int argc, char ** argv)

const auto cpu_node = std::make_shared<system_metrics_collector::LinuxCpuMeasurementNode>(
mm318 marked this conversation as resolved.
Show resolved Hide resolved
"linuxCpuCollector",
system_metrics_collector::collector_node_constants::kDefaultCollectPeriod,
system_metrics_collector::collector_node_constants::kStatisticsTopicName,
system_metrics_collector::collector_node_constants::kDefaultPublishPeriod);
rclcpp::NodeOptions());

rclcpp::executors::MultiThreadedExecutor ex;
cpu_node->Start();
Expand Down
Expand Up @@ -38,10 +38,8 @@ namespace system_metrics_collector

LinuxCpuMeasurementNode::LinuxCpuMeasurementNode(
const std::string & name,
const std::chrono::milliseconds measurement_period,
const std::string & topic,
const std::chrono::milliseconds publish_period)
: PeriodicMeasurementNode(name, measurement_period, topic, publish_period)
const rclcpp::NodeOptions & options)
: PeriodicMeasurementNode(name, options)
mm318 marked this conversation as resolved.
Show resolved Hide resolved
{}

bool LinuxCpuMeasurementNode::SetupStart()
Expand Down
Expand Up @@ -41,11 +41,7 @@ class LinuxCpuMeasurementNode : public system_metrics_collector::PeriodicMeasure
* @param topic the topic name used for publishing
* @param publish_period the period at which metrics are published. 0 ms means don't publish
*/
LinuxCpuMeasurementNode(
const std::string & name,
const std::chrono::milliseconds measurement_period,
const std::string & topic,
const std::chrono::milliseconds publish_period);
LinuxCpuMeasurementNode(const std::string & name, const rclcpp::NodeOptions & options);
dabonnie marked this conversation as resolved.
Show resolved Hide resolved

virtual ~LinuxCpuMeasurementNode() = default;

Expand Down
Expand Up @@ -30,9 +30,7 @@ int main(int argc, char ** argv)

const auto mem_node = std::make_shared<system_metrics_collector::LinuxMemoryMeasurementNode>(
"linuxMemoryCollector",
system_metrics_collector::collector_node_constants::kDefaultCollectPeriod,
system_metrics_collector::collector_node_constants::kStatisticsTopicName,
system_metrics_collector::collector_node_constants::kDefaultPublishPeriod);
rclcpp::NodeOptions());

rclcpp::executors::MultiThreadedExecutor ex;
mem_node->Start();
Expand Down
Expand Up @@ -38,10 +38,8 @@ namespace system_metrics_collector

LinuxMemoryMeasurementNode::LinuxMemoryMeasurementNode(
const std::string & name,
const std::chrono::milliseconds measurement_period,
const std::string & topic,
const std::chrono::milliseconds publish_period)
: PeriodicMeasurementNode(name, measurement_period, topic, publish_period)
const rclcpp::NodeOptions & options)
: PeriodicMeasurementNode(name, options)
mm318 marked this conversation as resolved.
Show resolved Hide resolved
{
}

Expand Down
Expand Up @@ -38,11 +38,7 @@ class LinuxMemoryMeasurementNode : public system_metrics_collector::PeriodicMeas
* @param topic the topic name used for publishing
* @param publish_period the period at which metrics are published. 0 ms means don't publish
*/
LinuxMemoryMeasurementNode(
const std::string & name,
const std::chrono::milliseconds measurement_period,
const std::string & topic,
const std::chrono::milliseconds publish_period);
LinuxMemoryMeasurementNode(const std::string & name, const rclcpp::NodeOptions & options);
dabonnie marked this conversation as resolved.
Show resolved Hide resolved

virtual ~LinuxMemoryMeasurementNode() = default;

Expand Down
Expand Up @@ -41,10 +41,8 @@ namespace system_metrics_collector

LinuxProcessCpuMeasurementNode::LinuxProcessCpuMeasurementNode(
const std::string & name,
const std::chrono::milliseconds measurement_period,
const std::string & topic,
const std::chrono::milliseconds publish_period)
: PeriodicMeasurementNode(name, measurement_period, topic, publish_period),
const rclcpp::NodeOptions & options)
: PeriodicMeasurementNode(name, options),
mm318 marked this conversation as resolved.
Show resolved Hide resolved
metric_name_(std::to_string(GetPid()) + kMetricName)
mm318 marked this conversation as resolved.
Show resolved Hide resolved
{
}
Expand Down
Expand Up @@ -45,11 +45,7 @@ class LinuxProcessCpuMeasurementNode : public PeriodicMeasurementNode
* @param topic the topic name used for publishing
* @param publish_period the period at which metrics are published.
*/
LinuxProcessCpuMeasurementNode(
const std::string & name,
const std::chrono::milliseconds measurement_period,
const std::string & topic,
const std::chrono::milliseconds publish_period);
LinuxProcessCpuMeasurementNode(const std::string & name, const rclcpp::NodeOptions & options);

protected:
/**
Expand Down
Expand Up @@ -52,10 +52,8 @@ namespace system_metrics_collector

LinuxProcessMemoryMeasurementNode::LinuxProcessMemoryMeasurementNode(
const std::string & name,
const std::chrono::milliseconds measurement_period,
const std::string & topic,
const std::chrono::milliseconds publish_period)
: PeriodicMeasurementNode(name, measurement_period, topic, publish_period),
const rclcpp::NodeOptions & options)
: PeriodicMeasurementNode(name, options),
mm318 marked this conversation as resolved.
Show resolved Hide resolved
pid_(std::to_string(GetPid())),
mm318 marked this conversation as resolved.
Show resolved Hide resolved
file_to_read_(kProc + std::to_string(GetPid()) + kStatm)
mm318 marked this conversation as resolved.
Show resolved Hide resolved
{
Expand Down
Expand Up @@ -52,11 +52,7 @@ class LinuxProcessMemoryMeasurementNode : public PeriodicMeasurementNode
* @param topic the topic name used for publishing
* @param publish_period the period at which metrics are published.
*/
LinuxProcessMemoryMeasurementNode(
const std::string & name,
const std::chrono::milliseconds measurement_period,
const std::string & topic,
const std::chrono::milliseconds publish_period);
LinuxProcessMemoryMeasurementNode(const std::string & name, const rclcpp::NodeOptions & options);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Same comment concerning the doc

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done.


protected:
/**
Expand Down
22 changes: 4 additions & 18 deletions system_metrics_collector/src/system_metrics_collector/main.cpp
Expand Up @@ -27,12 +27,6 @@
#include "../../src/system_metrics_collector/linux_process_cpu_measurement_node.hpp"
#include "../../src/system_metrics_collector/linux_process_memory_measurement_node.hpp"

namespace
{
constexpr const char kStatisticsTopicName[] = "system_metrics";
constexpr const std::chrono::seconds kDefaultCollectPeriod{1};
constexpr const std::chrono::minutes kDefaultPublishPeriod{1};
} // namespace

void set_node_to_debug(
const system_metrics_collector::PeriodicMeasurementNode * node,
Expand All @@ -59,29 +53,21 @@ int main(int argc, char ** argv)
using namespace std::chrono_literals;
const auto cpu_node = std::make_shared<system_metrics_collector::LinuxCpuMeasurementNode>(
"linuxCpuCollector",
kDefaultCollectPeriod,
kStatisticsTopicName,
kDefaultPublishPeriod);
rclcpp::NodeOptions());

const auto mem_node = std::make_shared<system_metrics_collector::LinuxMemoryMeasurementNode>(
"linuxMemoryCollector",
kDefaultCollectPeriod,
kStatisticsTopicName,
kDefaultPublishPeriod);
rclcpp::NodeOptions());
mm318 marked this conversation as resolved.
Show resolved Hide resolved

const auto process_cpu_node =
std::make_shared<system_metrics_collector::LinuxProcessCpuMeasurementNode>(
"linuxProcessCpuCollector",
kDefaultCollectPeriod,
kStatisticsTopicName,
kDefaultPublishPeriod);
rclcpp::NodeOptions());

const auto process_mem_node =
std::make_shared<system_metrics_collector::LinuxProcessMemoryMeasurementNode>(
"linuxProcessMemoryCollector",
kDefaultCollectPeriod,
kStatisticsTopicName,
kDefaultPublishPeriod);
rclcpp::NodeOptions());

rclcpp::executors::MultiThreadedExecutor ex;
cpu_node->Start();
Expand Down
Expand Up @@ -16,9 +16,11 @@
#include "periodic_measurement_node.hpp"

#include <chrono>
#include <limits>
#include <stdexcept>
#include <string>

#include "constants.hpp"
#include "rclcpp/rclcpp.hpp"

using metrics_statistics_msgs::msg::MetricsMessage;
Expand All @@ -28,34 +30,50 @@ namespace system_metrics_collector

PeriodicMeasurementNode::PeriodicMeasurementNode(
const std::string & name,
const std::chrono::milliseconds measurement_period,
const std::string & publishing_topic,
const std::chrono::milliseconds publish_period)
: Node(name),
measurement_period_(measurement_period),
publishing_topic_(publishing_topic),
measurement_timer_(nullptr),
publish_timer_(nullptr),
publish_period_(publish_period)
const rclcpp::NodeOptions & options)
: Node(name, options)
mm318 marked this conversation as resolved.
Show resolved Hide resolved
{
rcl_interfaces::msg::IntegerRange positive_range;
positive_range.from_value = 1;
positive_range.to_value =
std::numeric_limits<decltype(rcl_interfaces::msg::IntegerRange::to_value)>::max();
positive_range.step = 1;

rcl_interfaces::msg::ParameterDescriptor descriptor;
descriptor.read_only = true;
descriptor.integer_range.push_back(positive_range);
mm318 marked this conversation as resolved.
Show resolved Hide resolved

// rclcpp::Node throws if name is empty

if (measurement_period <= std::chrono::milliseconds{0}) {
descriptor.description = "The period in milliseconds between each measurement";
auto measurement_period = declare_parameter(
collector_node_constants::kCollectPeriodParam,
collector_node_constants::kDefaultCollectPeriodInMs,
descriptor);
measurement_period_ = std::chrono::milliseconds{measurement_period};
if (measurement_period_ <= std::chrono::milliseconds{0}) {
throw std::invalid_argument{"measurement_period cannot be negative"};
}
mm318 marked this conversation as resolved.
Show resolved Hide resolved

if (publishing_topic.empty()) {
throw std::invalid_argument{"publishing_topic cannot be empty"};
}

if (publish_period <= std::chrono::milliseconds{0}) {
descriptor.description = "The period in milliseconds between each published MetricsMessage";
auto publish_period = declare_parameter(
collector_node_constants::kPublishPeriodParam,
collector_node_constants::kDefaultPublishPeriodInMs,
descriptor);
publish_period_ = std::chrono::milliseconds{publish_period};
if (publish_period_ <= std::chrono::milliseconds{0}) {
throw std::invalid_argument{"publish_period cannot be negative"};
}
mm318 marked this conversation as resolved.
Show resolved Hide resolved

if (publish_period <= measurement_period) {
if (publish_period_ <= measurement_period_) {
throw std::invalid_argument{
"publish_period cannot be less than or equal to the measurement_period"};
}

publishing_topic_ = collector_node_constants::kStatisticsTopicName;
if (publishing_topic_.empty()) {
throw std::invalid_argument{"publishing_topic cannot be empty"};
}
}

bool PeriodicMeasurementNode::SetupStart()
Expand Down Expand Up @@ -104,7 +122,7 @@ std::string PeriodicMeasurementNode::GetStatusString() const
std::stringstream ss;
ss << "name=" << get_name() <<
", measurement_period=" << std::to_string(measurement_period_.count()) << "ms" <<
", publishing_topic=" << publishing_topic_ <<
", publishing_topic=" << ((publisher_ == nullptr) ? "" : publisher_->get_topic_name()) <<
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The .hpp still has publishing_topic_. Why tie displaying the topic to the node state?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If displaying the topic doesn't depend on the node state, then when the user does topic name remapping, the output is technically wrong.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could this be simplified to publisher_ ? publisher_->get_topic_name() : ""

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done.

", publish_period=" << std::to_string(publish_period_.count()) + "ms" <<
", " << Collector::GetStatusString();
return ss.str();
Expand Down
Expand Up @@ -46,11 +46,7 @@ class PeriodicMeasurementNode : public system_metrics_collector::Collector,
* strictly greater than measurement_period.
* @throws std::invalid_argument for any invalid input
*/
PeriodicMeasurementNode(
const std::string & name,
const std::chrono::milliseconds measurement_period,
const std::string & publishing_topic, // todo @dbbonnie think about a default topic
const std::chrono::milliseconds publish_period);
PeriodicMeasurementNode(const std::string & name, const rclcpp::NodeOptions & options);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Doc string update

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done.


virtual ~PeriodicMeasurementNode() = default;

Expand Down Expand Up @@ -108,15 +104,15 @@ class PeriodicMeasurementNode : public system_metrics_collector::Collector,
/**
* Topic used for publishing
*/
const std::string publishing_topic_;
std::string publishing_topic_;
/**
* The period used to take a single measurement
*/
const std::chrono::milliseconds measurement_period_;
std::chrono::milliseconds measurement_period_;
/**
* The period used to publish measurement data
*/
const std::chrono::milliseconds publish_period_;
std::chrono::milliseconds publish_period_;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this used anymore?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The value is coming from the parameters and stored here. Is your idea that the parameters values should be read at Start() and not stored, so that each Stop()->Start() cycle might have updated/different parameter values?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sorry, this is the wrong line. I meant for line 107 (std::string publishing_topic_;). Mainly because there is a change with the status string that does not used publishing_topic_.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Okay, I removed the publishing_topic_ member variable.


rclcpp::TimerBase::SharedPtr measurement_timer_;
rclcpp::TimerBase::SharedPtr publish_timer_;
Expand Down
Expand Up @@ -22,10 +22,12 @@
#include <mutex>
#include <string>
#include <unordered_map>
#include <vector>

#include "metrics_statistics_msgs/msg/metrics_message.hpp"
#include "metrics_statistics_msgs/msg/statistic_data_type.hpp"

#include "../../src/system_metrics_collector/constants.hpp"
#include "../../src/system_metrics_collector/linux_cpu_measurement_node.hpp"
#include "../../src/system_metrics_collector/proc_cpu_data.hpp"
#include "../../src/system_metrics_collector/utilities.hpp"
Expand All @@ -50,12 +52,8 @@ constexpr const char kTestMetricName[] = "system_cpu_percent_used";
class TestLinuxCpuMeasurementNode : public system_metrics_collector::LinuxCpuMeasurementNode
{
public:
TestLinuxCpuMeasurementNode(
const std::string & name,
const std::chrono::milliseconds measurement_period,
const std::string & publishing_topic,
const std::chrono::milliseconds publish_period)
: LinuxCpuMeasurementNode(name, measurement_period, publishing_topic, publish_period) {}
TestLinuxCpuMeasurementNode(const std::string & name, const rclcpp::NodeOptions & options)
: LinuxCpuMeasurementNode(name, options) {}

~TestLinuxCpuMeasurementNode() override = default;

Expand Down Expand Up @@ -212,8 +210,21 @@ class LinuxCpuMeasurementTestFixture : public ::testing::Test
{
rclcpp::init(0, nullptr);

test_measure_linux_cpu_ = std::make_shared<TestLinuxCpuMeasurementNode>(kTestNodeName,
test_constants::kMeasurePeriod, kTestTopic, test_constants::kPublishPeriod);
rclcpp::NodeOptions options;
options.append_parameter_override(
system_metrics_collector::collector_node_constants::kCollectPeriodParam,
test_constants::kMeasurePeriod.count());
options.append_parameter_override(
system_metrics_collector::collector_node_constants::kPublishPeriodParam,
test_constants::kPublishPeriod.count());

std::vector<std::string> arguments = {"--ros-args", "--remap", std::string(
system_metrics_collector::collector_node_constants::kStatisticsTopicName) +
":=" + kTestTopic};
options.arguments(arguments);

test_measure_linux_cpu_ = std::make_shared<TestLinuxCpuMeasurementNode>(
kTestNodeName, options);

ASSERT_FALSE(test_measure_linux_cpu_->IsStarted());

Expand Down