1#include "modulo_components/ComponentInterface.hpp"
3#include <console_bridge/console.h>
4#include <tf2_msgs/msg/tf_message.hpp>
6#include <modulo_core/translators/message_readers.hpp>
7#include <modulo_core/translators/message_writers.hpp>
14 const std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>>& interfaces)
15 : node_base_(interfaces->get_node_base_interface()),
16 node_clock_(interfaces->get_node_clock_interface()),
17 node_logging_(interfaces->get_node_logging_interface()),
18 node_parameters_(interfaces->get_node_parameters_interface()),
19 node_services_(interfaces->get_node_services_interface()),
20 node_timers_(interfaces->get_node_timers_interface()),
21 node_topics_(interfaces->get_node_topics_interface()) {
23 this->pre_set_parameter_cb_handle_ = this->node_parameters_->add_pre_set_parameters_callback(
24 [
this](std::vector<rclcpp::Parameter>&
parameters) {
return this->pre_set_parameters_callback(
parameters); });
25 this->on_set_parameter_cb_handle_ = this->node_parameters_->add_on_set_parameters_callback(
26 [
this](
const std::vector<rclcpp::Parameter>&
parameters) -> rcl_interfaces::msg::SetParametersResult {
27 return this->on_set_parameters_callback(
parameters);
29 this->
add_parameter(
"rate", 10.0,
"The rate in Hertz for all periodic callbacks",
true);
31 this->predicate_publisher_ = rclcpp::create_publisher<modulo_interfaces::msg::PredicateCollection>(
32 this->node_parameters_, this->node_topics_,
"/predicates", this->qos_);
33 this->predicate_message_.node = this->node_base_->get_fully_qualified_name();
34 this->predicate_message_.type = modulo_interfaces::msg::PredicateCollection::COMPONENT;
39 this->period_ = 1.0 / this->rate_;
40 this->step_timer_ = rclcpp::create_wall_timer(
41 std::chrono::nanoseconds(
static_cast<int64_t>(1
e9 * this->period_)),
43 if (this->step_mutex_.try_lock()) {
45 this->step_mutex_.unlock();
48 nullptr, this->node_base_.get(), this->node_timers_.get());
52 this->step_mutex_.lock();
66 return std::chrono::nanoseconds(
static_cast<int64_t>(1
e9 * this->period_));
71 return rclcpp::Duration::from_seconds(this->period_);
79 const std::shared_ptr<state_representation::ParameterInterface>& parameter,
const std::string& description,
87 if (!this->node_parameters_->has_parameter(
parameter->get_name())) {
89 this->parameter_map_.set_parameter(
parameter);
90 this->read_only_parameters_.insert_or_assign(
parameter->get_name(),
false);
92 rcl_interfaces::msg::ParameterDescriptor
descriptor;
96 this->set_parameters_result_.successful =
true;
97 this->set_parameters_result_.reason =
"";
101 this->node_parameters_->declare_parameter(
parameter->get_name(), rclcpp::ParameterValue{},
descriptor);
105 std::vector<rclcpp::Parameter>
ros_parameters{this->node_parameters_->get_parameters({
parameter->get_name()})};
109 this->node_parameters_->undeclare_parameter(
parameter->get_name());
113 }
catch (
const std::exception&
ex) {
114 this->parameter_map_.remove_parameter(
parameter->get_name());
115 this->read_only_parameters_.erase(
parameter->get_name());
120 this->node_logging_->get_logger(),
"Parameter '" <<
parameter->get_name() <<
"' already exists.");
124std::shared_ptr<state_representation::ParameterInterface>
127 return this->parameter_map_.get_parameter(
name);
128 }
catch (
const state_representation::exceptions::InvalidParameterException&
ex) {
133void ComponentInterface::pre_set_parameters_callback(std::vector<rclcpp::Parameter>& parameters) {
134 if (this->pre_set_parameter_callback_called_) {
135 this->pre_set_parameter_callback_called_ =
false;
138 rcl_interfaces::msg::SetParametersResult
result;
143 if (this->read_only_parameters_.at(
ros_parameter.get_name())) {
145 this->node_logging_->get_logger(),
"Parameter '" <<
ros_parameter.get_name() <<
"' is read only.");
152 result.successful =
false;
153 result.reason +=
"Validation of parameter '" +
ros_parameter.get_name() +
"' returned false!";
159 }
catch (
const std::exception&
ex) {
160 result.successful =
false;
164 this->set_parameters_result_ =
result;
167rcl_interfaces::msg::SetParametersResult
168ComponentInterface::on_set_parameters_callback(
const std::vector<rclcpp::Parameter>&) {
169 auto result = this->set_parameters_result_;
170 this->set_parameters_result_.successful =
true;
171 this->set_parameters_result_.reason =
"";
175bool ComponentInterface::validate_parameter(
176 const std::shared_ptr<state_representation::ParameterInterface>& parameter) {
180 RCLCPP_ERROR(this->node_logging_->get_logger(),
"Value for parameter 'rate' has to be a positive finite number.");
188 const std::shared_ptr<state_representation::ParameterInterface>&) {
197 const std::string& predicate_name,
const std::function<
bool(
void)>& predicate_function) {
199 RCLCPP_ERROR(this->node_logging_->get_logger(),
"Failed to add predicate: Provide a non empty string as a name.");
204 this->node_logging_->get_logger(),
205 "Predicate with name '" <<
predicate_name <<
"' already exists, overwriting.");
211 }
catch (
const std::exception&
ex) {
213 this->node_logging_->get_logger(), *
this->node_clock_->get_clock(), 1000,
222 this->node_logging_->get_logger(), *
this->node_clock_->get_clock(), 1000,
223 "Failed to get predicate '" <<
predicate_name <<
"': Predicate does not exist, returning false.");
228 }
catch (
const std::exception&
ex) {
230 this->node_logging_->get_logger(),
231 "Failed to evaluate callback of predicate '" <<
predicate_it->first <<
"', returning false: " <<
ex.what());
241 const std::string& predicate_name,
const std::function<
bool(
void)>& predicate_function) {
245 this->node_logging_->get_logger(), *
this->node_clock_->get_clock(), 1000,
246 "Failed to set predicate '" <<
predicate_name <<
"': Predicate does not exist.");
257 RCLCPP_ERROR(this->node_logging_->get_logger(),
"Failed to add trigger: Provide a non empty string as a name.");
260 if (std::find(this->triggers_.cbegin(),
this->triggers_.cend(),
trigger_name) !=
this->triggers_.cend()) {
262 this->node_logging_->get_logger(),
263 "Failed to add trigger: there is already a trigger with name '" <<
trigger_name <<
"'.");
268 this->node_logging_->get_logger(),
269 "Failed to add trigger: there is already a predicate with name '" <<
trigger_name <<
"'.");
277 if (std::find(this->triggers_.cbegin(),
this->triggers_.cend(),
trigger_name) ==
this->triggers_.cend()) {
279 this->node_logging_->get_logger(),
280 "Failed to trigger: could not find trigger with name '" <<
trigger_name <<
"'.");
285 this->predicates_.at(
trigger_name).set_predicate([]() {
return false; });
289 const std::string& signal_name,
const std::string& default_topic,
bool fixed_topic) {
294 const std::string& signal_name,
const std::string& default_topic,
bool fixed_topic) {
298void ComponentInterface::declare_signal(
299 const std::string& signal_name,
const std::string& type,
const std::string& default_topic,
bool fixed_topic) {
306 this->node_logging_->get_logger(),
308 +
"'. Use the parsed name to refer to this " +
type);
310 if (this->
inputs_.find(parsed_signal_name) !=
this->inputs_.cend()) {
313 if (this->
outputs_.find(parsed_signal_name) !=
this->outputs_.cend()) {
325 this->node_logging_->get_logger(),
332 if (this->
outputs_.find(parsed_signal_name) ==
this->outputs_.cend()) {
339 this->
outputs_.at(parsed_signal_name)->publish();
342 this->node_logging_->get_logger(), *
this->node_clock_->get_clock(), 1000,
349 auto parsed_signal_name = modulo_utils::parsing::parse_topic_name(
signal_name);
352 this->node_logging_->get_logger(),
360 auto parsed_signal_name = modulo_utils::parsing::parse_topic_name(
signal_name);
363 this->node_logging_->get_logger(),
374 auto service = rclcpp::create_service<modulo_interfaces::srv::EmptyTrigger>(
377 const std::shared_ptr<modulo_interfaces::srv::EmptyTrigger::Request>,
378 std::shared_ptr<modulo_interfaces::srv::EmptyTrigger::Response>
response) {
383 }
catch (
const std::exception&
ex) {
388 this->qos_,
nullptr);
390 }
catch (
const std::exception&
ex) {
392 this->node_logging_->get_logger(),
"Failed to add service '" <<
service_name <<
"': " <<
ex.what());
397 const std::string& service_name,
402 auto service = rclcpp::create_service<modulo_interfaces::srv::StringTrigger>(
405 const std::shared_ptr<modulo_interfaces::srv::StringTrigger::Request>
request,
406 std::shared_ptr<modulo_interfaces::srv::StringTrigger::Response>
response) {
411 }
catch (
const std::exception&
ex) {
416 this->qos_,
nullptr);
418 }
catch (
const std::exception&
ex) {
420 this->node_logging_->get_logger(),
"Failed to add service '" <<
service_name <<
"': " <<
ex.what());
425std::string ComponentInterface::validate_service_name(
const std::string& service_name,
const std::string& type)
const {
429 modulo_utils::parsing::topic_validation_warning(
service_name,
type +
" service"));
433 this->node_logging_->get_logger(),
435 +
"'. Use the parsed name to refer to this service");
452 this->node_logging_->get_logger(),
"Failed to add periodic function: Provide a non empty string as a name.");
455 if (this->periodic_callbacks_.find(
name) !=
this->periodic_callbacks_.end()) {
457 this->node_logging_->get_logger(),
"Periodic function '" <<
name <<
"' already exists, overwriting.");
461 this->periodic_callbacks_.insert_or_assign(
name,
callback);
465 if (this->tf_broadcaster_ ==
nullptr) {
466 RCLCPP_DEBUG(this->node_logging_->get_logger(),
"Adding TF broadcaster.");
467 console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
468 this->tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(
469 this->node_parameters_, this->node_topics_, tf2_ros::DynamicBroadcasterQoS());
471 RCLCPP_DEBUG(this->node_logging_->get_logger(),
"TF broadcaster already exists.");
476 if (this->static_tf_broadcaster_ ==
nullptr) {
477 RCLCPP_DEBUG(this->node_logging_->get_logger(),
"Adding static TF broadcaster.");
478 console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
479 tf2_ros::StaticBroadcasterQoS
qos;
480 rclcpp::PublisherOptionsWithAllocator<std::allocator<void>>
options;
481 this->static_tf_broadcaster_ =
482 std::make_shared<tf2_ros::StaticTransformBroadcaster>(this->node_parameters_, this->node_topics_,
qos,
options);
484 RCLCPP_DEBUG(this->node_logging_->get_logger(),
"Static TF broadcaster already exists.");
489 if (this->tf_buffer_ ==
nullptr || this->tf_listener_ ==
nullptr) {
490 RCLCPP_DEBUG(this->node_logging_->get_logger(),
"Adding TF buffer and listener.");
491 console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
492 this->tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->node_clock_->get_clock());
493 this->tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*this->tf_buffer_);
495 RCLCPP_DEBUG(this->node_logging_->get_logger(),
"TF buffer and listener already exist.");
504 this->publish_transforms(
transforms, this->tf_broadcaster_);
512 this->publish_transforms(
transforms, this->static_tf_broadcaster_,
true);
516 const std::string& frame,
const std::string& reference_frame,
const tf2::TimePoint& time_point,
517 const tf2::Duration& duration) {
525 const std::string& frame,
const std::string& reference_frame,
double validity_period,
526 const tf2::Duration& duration) {
538geometry_msgs::msg::TransformStamped ComponentInterface::lookup_ros_transform(
539 const std::string& frame,
const std::string& reference_frame,
const tf2::TimePoint& time_point,
540 const tf2::Duration& duration) {
541 if (this->tf_buffer_ ==
nullptr || this->tf_listener_ ==
nullptr) {
546 }
catch (
const tf2::TransformException&
ex) {
560 RCLCPP_DEBUG(this->node_logging_->get_logger(),
"raise_error called: Setting predicate 'in_error_state' to true.");
564modulo_interfaces::msg::Predicate ComponentInterface::get_predicate_message(
const std::string& name,
bool value)
const {
565 modulo_interfaces::msg::Predicate message;
566 message.predicate =
name;
567 message.value =
value;
571void ComponentInterface::publish_predicate(
const std::string& name,
bool value)
const {
572 auto message(this->predicate_message_);
573 message.predicates.push_back(this->get_predicate_message(
name,
value));
574 this->predicate_publisher_->publish(message);
578 auto message(this->predicate_message_);
584 if (message.predicates.size()) {
585 this->predicate_publisher_->publish(message);
597 this->node_logging_->get_logger(), *
this->node_clock_->get_clock(), 1000,
598 "Failed to publish output '" <<
signal <<
"': " <<
ex.what());
604 for (
const auto& [
name,
callback] : this->periodic_callbacks_) {
607 }
catch (
const std::exception&
ex) {
609 this->node_logging_->get_logger(), *
this->node_clock_->get_clock(), 1000,
610 "Failed to evaluate periodic function callback '" <<
name <<
"': " <<
ex.what());
double get_rate() const
Get the component rate in Hertz.
void send_transforms(const std::vector< state_representation::CartesianPose > &transforms)
Send a vector of transforms to TF.
virtual bool on_validate_parameter_callback(const std::shared_ptr< state_representation::ParameterInterface > ¶meter)
Parameter validation function to be redefined by derived Component classes.
void publish_outputs()
Helper function to publish all output signals.
void remove_input(const std::string &signal_name)
Remove an input from the map of inputs.
void declare_input(const std::string &signal_name, const std::string &default_topic="", bool fixed_topic=false)
Declare an input to create the topic parameter without adding it to the map of inputs yet.
void declare_output(const std::string &signal_name, const std::string &default_topic="", bool fixed_topic=false)
Declare an output to create the topic parameter without adding it to the map of outputs yet.
T get_period() const
Get the component period.
void send_static_transforms(const std::vector< state_representation::CartesianPose > &transforms)
Send a vector of static transforms to TF.
std::map< std::string, std::shared_ptr< modulo_core::communication::PublisherInterface > > outputs_
Map of outputs.
std::shared_ptr< state_representation::ParameterInterface > get_parameter(const std::string &name) const
Get a parameter by name.
T get_parameter_value(const std::string &name) const
Get a parameter value by name.
virtual ~ComponentInterface()
Virtual destructor.
void add_static_tf_broadcaster()
Configure a static transform broadcaster.
void add_parameter(const std::shared_ptr< state_representation::ParameterInterface > ¶meter, const std::string &description, bool read_only=false)
Add a parameter.
void trigger(const std::string &trigger_name)
Latch the trigger with the provided name.
state_representation::CartesianPose lookup_transform(const std::string &frame, const std::string &reference_frame, const tf2::TimePoint &time_point, const tf2::Duration &duration)
Look up a transform from TF.
virtual void on_step_callback()
Steps to execute periodically. To be redefined by derived Component classes.
void set_predicate(const std::string &predicate_name, bool predicate_value)
Set the value of the predicate given as parameter, if the predicate is not found does not do anything...
virtual void raise_error()
Put the component in error state by setting the 'in_error_state' predicate to true.
ComponentInterface(const std::shared_ptr< rclcpp::node_interfaces::NodeInterfaces< ALL_RCLCPP_NODE_INTERFACES > > &interfaces)
Constructor with all node interfaces.
void remove_output(const std::string &signal_name)
Remove an output from the map of outputs.
void publish_output(const std::string &signal_name)
Trigger the publishing of an output.
virtual void step()
Step function that is called periodically.
void publish_predicates()
Helper function to publish all predicates.
void set_qos(const rclcpp::QoS &qos)
Set the Quality of Service for ROS publishers and subscribers.
rclcpp::QoS get_qos() const
Getter of the Quality of Service attribute.
void add_predicate(const std::string &predicate_name, bool predicate_value)
Add a predicate to the map of predicates.
void send_transform(const state_representation::CartesianPose &transform)
Send a transform to TF.
void add_trigger(const std::string &trigger_name)
Add a trigger to the component. Triggers are predicates that are always false except when it's trigge...
void add_periodic_callback(const std::string &name, const std::function< void(void)> &callback)
Add a periodic callback function.
std::map< std::string, std::shared_ptr< modulo_core::communication::SubscriptionInterface > > inputs_
Map of inputs.
void add_tf_listener()
Configure a transform buffer and listener.
void evaluate_periodic_callbacks()
Helper function to evaluate all periodic function callbacks.
void add_tf_broadcaster()
Configure a transform broadcaster.
void send_static_transform(const state_representation::CartesianPose &transform)
Send a static transform to TF.
bool get_predicate(const std::string &predicate_name) const
Get the logical value of a predicate.
std::map< std::string, bool > periodic_outputs_
Map of outputs with periodic publishing flag.
void add_service(const std::string &service_name, const std::function< ComponentServiceResponse(void)> &callback)
Add a service to trigger a callback function with no input arguments.
An exception class to notify errors when adding a service.
An exception class to notify errors when adding a signal.
A base class for all core exceptions.
An exception class to notify errors with parameters in modulo classes.
An exception class to notify incompatibility when translating parameters from different sources.
rclcpp::Parameter write_parameter(const std::shared_ptr< state_representation::ParameterInterface > ¶meter)
Write a ROS Parameter from a ParameterInterface pointer.
rclcpp::ParameterType get_ros_parameter_type(const state_representation::ParameterType ¶meter_type)
Given a state representation parameter type, get the corresponding ROS parameter type.
void copy_parameter_value(const std::shared_ptr< const state_representation::ParameterInterface > &source_parameter, const std::shared_ptr< state_representation::ParameterInterface > ¶meter)
Copy the value of one parameter interface into another.
void read_message(state_representation::CartesianState &state, const geometry_msgs::msg::Accel &message)
Convert a ROS geometry_msgs::msg::Accel to a CartesianState.
std::shared_ptr< state_representation::ParameterInterface > read_parameter_const(const rclcpp::Parameter &ros_parameter, const std::shared_ptr< const state_representation::ParameterInterface > ¶meter)
Update the parameter value of a ParameterInterface from a ROS Parameter object only if the two parame...
Response structure to be returned by component services.