Use rclcpp::NodeOptions during node construction #73
Changes from 7 commits
f4ad662
c13910b
27309ec
e82645e
caa445f
d7cc0fa
7a419c1
9d770d2
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -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; | ||
|
@@ -28,31 +30,36 @@ 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} | ||
{ | ||
// rclcpp::Node throws if name is empty | ||
|
||
if (measurement_period <= std::chrono::milliseconds{0}) { | ||
throw std::invalid_argument{"measurement_period cannot be negative"}; | ||
} | ||
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; | ||
|
||
if (publishing_topic.empty()) { | ||
throw std::invalid_argument{"publishing_topic cannot be empty"}; | ||
} | ||
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
|
||
|
||
if (publish_period <= std::chrono::milliseconds{0}) { | ||
throw std::invalid_argument{"publish_period cannot be negative"}; | ||
} | ||
// rclcpp::Node throws if name is empty | ||
|
||
if (publish_period <= measurement_period) { | ||
descriptor.description = "The period in milliseconds between each measurement"; | ||
auto measurement_period = declare_parameter( | ||
collector_node_constants::kCollectPeriodParam, | ||
collector_node_constants::kDefaultCollectPeriod.count(), | ||
descriptor); | ||
measurement_period_ = std::chrono::milliseconds{measurement_period}; | ||
|
||
descriptor.description = "The period in milliseconds between each published MetricsMessage"; | ||
auto publish_period = declare_parameter( | ||
collector_node_constants::kPublishPeriodParam, | ||
collector_node_constants::kDefaultPublishPeriod.count(), | ||
descriptor); | ||
publish_period_ = std::chrono::milliseconds{publish_period}; | ||
|
||
if (publish_period_ <= measurement_period_) { | ||
throw std::invalid_argument{ | ||
"publish_period cannot be less than or equal to the measurement_period"}; | ||
} | ||
|
@@ -68,7 +75,8 @@ bool PeriodicMeasurementNode::SetupStart() | |
measurement_period_, [this]() {this->PerformPeriodicMeasurement();}); | ||
|
||
if (publisher_ == nullptr) { | ||
publisher_ = create_publisher<MetricsMessage>(publishing_topic_, 10 /*history_depth*/); | ||
publisher_ = create_publisher<MetricsMessage>(collector_node_constants::kStatisticsTopicName, | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Oh I see what you're doing now. Why aren't we keeping the member? This PR is actually removing functionality where we could pull the publishing topic out of NodeOptions. The work required seems minimal and I don't see why we can't do it, as a read only parameter, here. There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
I was going to do that, too, but this is what @thomas-moulard suggested under on the same rationale as the node name. There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I took @thomas-moulard 's comment to mean we don't need to set and test a custom topic name in test. I'm going to follow up with him as we've discussed allowing a user to configure the topic name. There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This was discussed in the team chatroom. Topic name will be configured via remapping ( There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
Got it, found this explanation: https://index.ros.org/doc/ros2/Tutorials/Node-arguments/#example |
||
10 /*history_depth*/); | ||
} | ||
|
||
RCLCPP_DEBUG(this->get_logger(), "SetupStart: creating publish_timer_"); | ||
|
@@ -104,7 +112,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_ ? publisher_->get_topic_name() : "") << | ||
", publish_period=" << std::to_string(publish_period_.count()) + "ms" << | ||
", " << Collector::GetStatusString(); | ||
return ss.str(); | ||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -38,19 +38,15 @@ class PeriodicMeasurementNode : public system_metrics_collector::Collector, | |
public: | ||
/** | ||
* Construct a PeriodicMeasurementNode. | ||
* The following parameters may be set via the rclcpp::NodeOptions: | ||
* `measurement_period`: the period of this node, used to read measurements | ||
* `publish_period`: the period at which metrics are published | ||
* | ||
* @param name the name of this node. This must be non-empty. | ||
* @param topic the topic for publishing data. This must be non-empty. | ||
* @param measurement_period. This must be non-negative and strictly less than publish_period. | ||
* @param publish_period the window of active measurements. This must be non-negative and | ||
* strictly greater than measurement_period. | ||
* @param options the options (arguments, parameters, etc.) for this node | ||
* @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); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Doc string update There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Done. |
||
|
||
virtual ~PeriodicMeasurementNode() = default; | ||
|
||
|
@@ -105,18 +101,14 @@ class PeriodicMeasurementNode : public system_metrics_collector::Collector, | |
*/ | ||
void PublishStatisticMessage() override; | ||
|
||
/** | ||
* Topic used for publishing | ||
*/ | ||
const 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_; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Is this used anymore? There was a problem hiding this comment. Choose a reason for hiding this commentThe 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 There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Sorry, this is the wrong line. I meant for line 107 ( There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Okay, I removed the |
||
|
||
rclcpp::TimerBase::SharedPtr measurement_timer_; | ||
rclcpp::TimerBase::SharedPtr publish_timer_; | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why is this still in milliseconds when the delay is now 1 second?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I need to get the
count()
in milliseconds at some point.