Skip to content
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
1 change: 1 addition & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ Release Versions:
- refactor: move exceptions to modulo_utils (#90)
- refactor: remove modulo_component_interfaces (#88)
- feat: add add_interfaces function for derived controllers (#102)
- refactor: remove period parameter and make rate double (#108)

## 4.2.1

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,8 @@
"display_name": "Rate",
"description": "The frequency in Hertz for all periodic callbacks",
"parameter_name": "rate",
"parameter_type": "int",
"default_value": "10"
},
{
"display_name": "Period",
"description": "The time interval in seconds for all periodic callbacks. This parameter is deprecated and will be removed in the next major release. The rate parameter takes precedence and overrides the component period.",
"parameter_name": "period",
"parameter_type": "double",
"default_value": "0.1",
"internal": true
"default_value": "10.0"
}
],
"predicates": [
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,8 @@
"display_name": "Rate",
"description": "The frequency in Hertz for all periodic callbacks",
"parameter_name": "rate",
"parameter_type": "int",
"default_value": "10"
},
{
"display_name": "Period",
"description": "The time interval in seconds for all periodic callbacks. This parameter is deprecated and will be removed in the next major release. The rate parameter takes precedence and overrides the component period.",
"parameter_name": "period",
"parameter_type": "double",
"default_value": "0.1",
"internal": true
"default_value": "10.0"
}
],
"predicates": [
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,6 @@
#include <modulo_utils/parsing.hpp>
#include <modulo_utils/predicate_variant.hpp>

#include "modulo_components/utilities/utilities.hpp"

/**
* @namespace modulo_components
* @brief Modulo components
Expand Down Expand Up @@ -73,6 +71,19 @@ class ComponentInterface {
const std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>>& interfaces
);

/**
* @brief Get the component rate in Hertz
* @return The component rate
*/
double get_rate() const;

/**
* @brief Get the component period
* @return The component period
*/
template<typename T>
T get_period() const;

/**
* @brief Step function that is called periodically.
*/
Expand Down Expand Up @@ -544,6 +555,8 @@ class ComponentInterface {
const tf2::Duration& duration
);

double rate_; ///< The component rate in Hz
double period_; ///< The componet period in s
std::mutex step_mutex_; ///< Mutex for step callback

std::map<std::string, modulo_utils::PredicateVariant> predicates_; ///< Map of predicates
Expand Down

This file was deleted.

32 changes: 20 additions & 12 deletions source/modulo_components/modulo_components/component_interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@
from modulo_interfaces.srv import EmptyTrigger, StringTrigger
from modulo_utils.exceptions import AddServiceError, AddSignalError, ModuloError, ParameterError, LookupTransformError
from modulo_utils.parsing import parse_topic_name
from modulo_components.utilities import modify_parameter_overrides
from modulo_core import EncodedState
from modulo_core.exceptions import MessageTranslationError, ParameterTranslationError
from modulo_core.translators.parameter_translators import get_ros_parameter_type, read_parameter_const, write_parameter
Expand Down Expand Up @@ -44,8 +43,6 @@ class ComponentInterface(Node):

def __init__(self, node_name: str, *args, **kwargs):
node_kwargs = {key: value for key, value in kwargs.items() if key in NODE_KWARGS}
if "parameter_overrides" in node_kwargs.keys():
node_kwargs["parameter_overrides"] = modify_parameter_overrides(node_kwargs["parameter_overrides"])
super().__init__(node_name, *args, **node_kwargs)
self.__step_lock = Lock()
self._parameter_dict: Dict[str, Union[str, sr.Parameter]] = {}
Expand All @@ -64,22 +61,38 @@ def __init__(self, node_name: str, *args, **kwargs):
self._qos = QoSProfile(depth=10)

self.add_on_set_parameters_callback(self.__on_set_parameters_callback)
self.add_parameter(sr.Parameter("rate", 10, sr.ParameterType.INT),
self.add_parameter(sr.Parameter("rate", 10.0, sr.ParameterType.DOUBLE),
"The rate in Hertz for all periodic callbacks")
self.add_parameter(sr.Parameter("period", 0.1, sr.ParameterType.DOUBLE),
"The time interval in seconds for all periodic callbacks")

self._predicate_publisher = self.create_publisher(PredicateCollection, "/predicates", self._qos)
self.__predicate_message = PredicateCollection()
self.__predicate_message.node = self.get_fully_qualified_name()
self.__predicate_message.type = PredicateCollection.COMPONENT
self.add_predicate("in_error_state", False)

self.create_timer(self.get_parameter_value("period"), self.__step_with_mutex)
self._rate = self.get_parameter_value("rate")
self._period = 1.0 / self._rate
self.create_timer(self._period, self.__step_with_mutex)

def __del__(self):
self.__step_lock.acquire()

def get_rate(self):
"""
Get the component rate in Hertz.

:return: The component rate
"""
return self._rate

def get_period(self):
"""
Get the component period in seconds.

:return: The component period
"""
return self._period

def __step_with_mutex(self):
if self.__step_lock.acquire(blocking=False):
self._step()
Expand Down Expand Up @@ -223,11 +236,6 @@ def __validate_parameter(self, parameter: sr.Parameter) -> bool:
if value <= 0 or value > sys.float_info.max:
self.get_logger().error("Value for parameter 'rate' has to be a positive finite number.")
return False
if parameter.get_name() == "period":
value = parameter.get_value()
if value <= 0.0 or value > sys.float_info.max:
self.get_logger().error("Value for parameter 'period' has to be a positive finite number.")
return False
return self.on_validate_parameter_callback(parameter)

def on_validate_parameter_callback(self, parameter: sr.Parameter) -> bool:
Expand Down

This file was deleted.

36 changes: 0 additions & 36 deletions source/modulo_components/modulo_components/utilities/utilities.py

This file was deleted.

3 changes: 1 addition & 2 deletions source/modulo_components/src/Component.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,7 @@ using namespace rclcpp;
namespace modulo_components {

Component::Component(const NodeOptions& node_options, const std::string& fallback_name)
: Node(
modulo_utils::parse_node_name(node_options, fallback_name), utilities::modify_parameter_overrides(node_options)),
: Node(modulo_utils::parse_node_name(node_options, fallback_name), node_options),
ComponentInterface(std::make_shared<node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>>(
Node::get_node_base_interface(), Node::get_node_clock_interface(), Node::get_node_graph_interface(),
Node::get_node_logging_interface(), Node::get_node_parameters_interface(),
Expand Down
41 changes: 26 additions & 15 deletions source/modulo_components/src/ComponentInterface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,8 @@
#include <modulo_core/exceptions/ParameterTranslationException.hpp>
#include <modulo_core/translators/message_readers.hpp>
#include <modulo_core/translators/message_writers.hpp>

#include "modulo_utils/exceptions/AddServiceException.hpp"
#include "modulo_utils/exceptions/LookupTransformException.hpp"
#include <modulo_utils/exceptions/AddServiceException.hpp>
#include <modulo_utils/exceptions/LookupTransformException.hpp>

using namespace modulo_utils::exceptions;

Expand All @@ -29,8 +28,7 @@ ComponentInterface::ComponentInterface(
[this](const std::vector<rclcpp::Parameter>& parameters) -> rcl_interfaces::msg::SetParametersResult {
return this->on_set_parameters_callback(parameters);
});
this->add_parameter("rate", 10, "The rate in Hertz for all periodic callbacks", true);
this->add_parameter("period", 0.1, "The time interval in seconds for all periodic callbacks", true);
this->add_parameter("rate", 10.0, "The rate in Hertz for all periodic callbacks", true);

this->predicate_publisher_ = rclcpp::create_publisher<modulo_interfaces::msg::PredicateCollection>(
this->node_parameters_, this->node_topics_, "/predicates", this->qos_);
Expand All @@ -39,8 +37,10 @@ ComponentInterface::ComponentInterface(

this->add_predicate("in_error_state", false);

this->rate_ = this->get_parameter_value<double>("rate");
this->period_ = 1.0 / this->rate_;
Copy link

@bpapaspyros bpapaspyros May 29, 2024

Choose a reason for hiding this comment

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

Do we really need to have a member variable for the period? As in, couldn't we just call the get_period() function which in C++ could be a constexpr 1 / rate_. Clearly this is not a mistake, just trying to understand if it's for ease of use in derived classes or serves another purpose.

Same goes for the python code.

Copy link
Member Author

Choose a reason for hiding this comment

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

I wanted to save one operation...No there is no other purpose, I will remove it 👍

Copy link

@bpapaspyros bpapaspyros May 29, 2024

Choose a reason for hiding this comment

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

no strong opinion on this honestly. And I do hear the argumentation of saving one operation. The reason I asked is because we rarely use period in our derived classes.

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 reason I asked is because we rarely use period in our derived classes.

We have several motion components that are interested in the component period for sinusoidal motion or to keep track of elapsed time. At the end, it's all about providing the helpers to simplify the work of the users.

Choose a reason for hiding this comment

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

You are right yes. Then let's leave it and resolve this question. Thank you!

this->step_timer_ = rclcpp::create_wall_timer(
std::chrono::nanoseconds(static_cast<int64_t>(this->get_parameter_value<double>("period") * 1e9)),
std::chrono::nanoseconds(static_cast<int64_t>(1e9 * this->period_)),
[this] {
if (this->step_mutex_.try_lock()) {
this->step();
Expand All @@ -54,6 +54,25 @@ ComponentInterface::~ComponentInterface() {
this->step_mutex_.lock();
}

double ComponentInterface::get_rate() const {
return this->rate_;
}

template<>
double ComponentInterface::get_period() const {
return this->period_;
}

template<>
std::chrono::nanoseconds ComponentInterface::get_period() const {
return std::chrono::nanoseconds(static_cast<int64_t>(1e9 * this->period_));
}

template<>
rclcpp::Duration ComponentInterface::get_period() const {
return rclcpp::Duration::from_seconds(this->period_);
}

void ComponentInterface::step() {}

void ComponentInterface::on_step_callback() {}
Expand Down Expand Up @@ -143,20 +162,12 @@ ComponentInterface::on_set_parameters_callback(const std::vector<rclcpp::Paramet
bool
ComponentInterface::validate_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter) {
if (parameter->get_name() == "rate") {
auto value = parameter->get_parameter_value<int>();
auto value = parameter->get_parameter_value<double>();
if (value <= 0 || !std::isfinite(value)) {
RCLCPP_ERROR(this->node_logging_->get_logger(), "Value for parameter 'rate' has to be a positive finite number.");
return false;
}
}
if (parameter->get_name() == "period") {
auto value = parameter->get_parameter_value<double>();
if (value <= 0.0 || !std::isfinite(value)) {
RCLCPP_ERROR(this->node_logging_->get_logger(),
"Value for parameter 'period' has to be a positive finite number.");
return false;
}
}
return this->on_validate_parameter_callback(parameter);
}

Expand Down
5 changes: 2 additions & 3 deletions source/modulo_components/src/LifecycleComponent.cpp
Original file line number Diff line number Diff line change
@@ -1,15 +1,14 @@
#include "modulo_components/LifecycleComponent.hpp"

#include "modulo_core/exceptions/CoreException.hpp"
#include <modulo_core/exceptions/CoreException.hpp>

using namespace modulo_core::communication;
using namespace rclcpp_lifecycle;

namespace modulo_components {

LifecycleComponent::LifecycleComponent(const rclcpp::NodeOptions& node_options, const std::string& fallback_name)
: LifecycleNode(
modulo_utils::parse_node_name(node_options, fallback_name), utilities::modify_parameter_overrides(node_options)),
: LifecycleNode(modulo_utils::parse_node_name(node_options, fallback_name), node_options),
ComponentInterface(std::make_shared<rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>>(
LifecycleNode::get_node_base_interface(), LifecycleNode::get_node_clock_interface(),
LifecycleNode::get_node_graph_interface(), LifecycleNode::get_node_logging_interface(),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,8 @@ class ComponentPublicInterface : public Component {
const rclcpp::NodeOptions& node_options, const std::string& fallback_name = "ComponentPublicInterface"
) : Component(node_options, fallback_name) {}
using ComponentInterface::get_parameter_value;
using ComponentInterface::get_rate;
using ComponentInterface::get_period;
using Component::add_output;
using Component::outputs_;
using Component::periodic_outputs_;
Expand All @@ -98,6 +100,8 @@ class LifecycleComponentPublicInterface : public LifecycleComponent {
explicit LifecycleComponentPublicInterface(const rclcpp::NodeOptions& node_options) :
LifecycleComponent(node_options) {}
using ComponentInterface::get_parameter_value;
using ComponentInterface::get_rate;
using ComponentInterface::get_period;
using LifecycleComponent::add_output;
using LifecycleComponent::configure_outputs;
using LifecycleComponent::activate_outputs;
Expand Down
Loading