3#include <rclcpp/node_interfaces/node_interfaces.hpp>
5#include <tf2_ros/buffer.h>
6#include <tf2_ros/static_transform_broadcaster.h>
7#include <tf2_ros/transform_broadcaster.h>
8#include <tf2_ros/transform_listener.h>
10#include <state_representation/parameters/ParameterMap.hpp>
12#include <modulo_core/Predicate.hpp>
13#include <modulo_core/communication/PublisherHandler.hpp>
14#include <modulo_core/communication/PublisherType.hpp>
15#include <modulo_core/communication/SubscriptionHandler.hpp>
16#include <modulo_core/concepts.hpp>
17#include <modulo_core/exceptions.hpp>
18#include <modulo_core/translators/parameter_translators.hpp>
20#include <modulo_interfaces/msg/predicate_collection.hpp>
21#include <modulo_interfaces/srv/empty_trigger.hpp>
22#include <modulo_interfaces/srv/string_trigger.hpp>
24#include <modulo_utils/parsing.hpp>
71 const std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>>&
interfaces);
106 const std::shared_ptr<state_representation::ParameterInterface>&
parameter,
const std::string&
description,
246 template<
typename DataT>
260 template<
typename DataT>
273 template<
typename MsgT>
275 const std::string&
signal_name,
const std::function<
void(
const std::shared_ptr<MsgT>)>&
callback,
290 template<
typename DataT>
406 const tf2::Duration&
duration = tf2::Duration(std::chrono::microseconds(10)));
440 std::map<std::string, std::shared_ptr<modulo_core::communication::SubscriptionInterface>>
inputs_;
441 std::map<std::string, std::shared_ptr<modulo_core::communication::PublisherInterface>>
outputs_;
449 void pre_set_parameters_callback(std::vector<rclcpp::Parameter>&
parameters);
457 rcl_interfaces::msg::SetParametersResult on_set_parameters_callback(
const std::vector<rclcpp::Parameter>&
parameters);
466 bool validate_parameter(
const std::shared_ptr<state_representation::ParameterInterface>&
parameter);
473 modulo_interfaces::msg::Predicate get_predicate_message(
const std::string&
name,
bool value)
const;
507 std::string validate_service_name(
const std::string&
service_name,
const std::string&
type)
const;
524 void publish_transforms(
538 [[
nodiscard]] geometry_msgs::msg::TransformStamped lookup_ros_transform(
544 std::mutex step_mutex_;
546 std::map<std::string, modulo_core::Predicate> predicates_;
547 std::shared_ptr<rclcpp::Publisher<modulo_interfaces::msg::PredicateCollection>>
548 predicate_publisher_;
549 modulo_interfaces::msg::PredicateCollection predicate_message_;
550 std::vector<std::string> triggers_;
552 std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::EmptyTrigger>>>
554 std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::StringTrigger>>>
557 std::map<std::string, std::function<
void(
void)>> periodic_callbacks_;
559 state_representation::ParameterMap parameter_map_;
560 std::unordered_map<std::string, bool> read_only_parameters_;
561 std::shared_ptr<rclcpp::node_interfaces::PreSetParametersCallbackHandle>
562 pre_set_parameter_cb_handle_;
563 std::shared_ptr<rclcpp::node_interfaces::OnSetParametersCallbackHandle>
564 on_set_parameter_cb_handle_;
565 rcl_interfaces::msg::SetParametersResult set_parameters_result_;
566 bool pre_set_parameter_callback_called_ =
false;
568 std::shared_ptr<rclcpp::TimerBase> step_timer_;
569 std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
570 std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
571 std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
572 std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
574 std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;
575 std::shared_ptr<rclcpp::node_interfaces::NodeClockInterface> node_clock_;
576 std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> node_logging_;
577 std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> node_parameters_;
578 std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services_;
579 std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface> node_timers_;
580 std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> node_topics_;
581 rclcpp::QoS qos_ = rclcpp::QoS(10);
586 const std::string& name,
const T& value,
const std::string& description,
bool read_only) {
588 RCLCPP_ERROR(this->node_logging_->get_logger(),
"Failed to add parameter: Provide a non empty string as a name.");
598 }
catch (
const state_representation::exceptions::InvalidParameterException&
ex) {
600 "Failed to get parameter value of parameter '" +
name +
"': " +
ex.what());
609 this->pre_set_parameters_callback(
parameters);
610 this->pre_set_parameter_callback_called_ =
true;
614 this->node_logging_->get_logger(), *
this->node_clock_->get_clock(), 1000,
615 "Failed to set parameter value of parameter '" <<
name <<
"': " <<
result.reason);
617 }
catch (
const std::exception&
ex) {
619 this->node_logging_->get_logger(), *
this->node_clock_->get_clock(), 1000,
620 "Failed to set parameter value of parameter '" <<
name <<
"': " <<
ex.what());
624template<
typename DataT>
626 const std::string& signal_name,
const std::shared_ptr<DataT>& data,
const std::string& default_topic,
631template<
typename DataT>
633 const std::string& signal_name,
const std::shared_ptr<DataT>& data,
const std::function<
void()>& user_callback,
634 const std::string& default_topic,
bool fixed_topic) {
637 if (
data ==
nullptr) {
644 this->node_logging_->get_logger(),
646 auto message_pair = make_shared_message_pair(
data, this->node_clock_->get_clock());
649 case MessageType::BOOL: {
651 std::make_shared<SubscriptionHandler<std_msgs::msg::Bool>>(
message_pair, this->node_logging_->get_logger());
652 auto subscription = rclcpp::create_subscription<std_msgs::msg::Bool>(
653 this->node_parameters_, this->node_topics_,
topic_name, this->qos_,
658 case MessageType::FLOAT64: {
661 auto subscription = rclcpp::create_subscription<std_msgs::msg::Float64>(
662 this->node_parameters_, this->node_topics_,
topic_name, this->qos_,
667 case MessageType::FLOAT64_MULTI_ARRAY: {
668 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::Float64MultiArray>>(
670 auto subscription = rclcpp::create_subscription<std_msgs::msg::Float64MultiArray>(
671 this->node_parameters_, this->node_topics_,
topic_name, this->qos_,
676 case MessageType::INT32: {
679 auto subscription = rclcpp::create_subscription<std_msgs::msg::Int32>(
680 this->node_parameters_, this->node_topics_,
topic_name, this->qos_,
685 case MessageType::STRING: {
688 auto subscription = rclcpp::create_subscription<std_msgs::msg::String>(
689 this->node_parameters_, this->node_topics_,
topic_name, this->qos_,
694 case MessageType::ENCODED_STATE: {
697 auto subscription = rclcpp::create_subscription<modulo_core::EncodedState>(
698 this->node_parameters_, this->node_topics_,
topic_name, this->qos_,
703 case MessageType::CUSTOM_MESSAGE: {
706 std::make_shared<SubscriptionHandler<DataT>>(
message_pair, this->node_logging_->get_logger());
708 this->node_parameters_, this->node_topics_,
topic_name, this->qos_,
716 }
catch (
const std::exception&
ex) {
718 this->node_logging_->get_logger(),
"Failed to add input '" <<
signal_name <<
"': " <<
ex.what());
722template<
typename MsgT>
724 const std::string& signal_name,
const std::function<
void(
const std::shared_ptr<MsgT>)>& callback,
725 const std::string& default_topic,
bool fixed_topic) {
732 this->node_logging_->get_logger(),
735 rclcpp::create_subscription<MsgT>(this->node_parameters_, this->node_topics_,
topic_name, this->qos_,
callback);
737 std::make_shared<SubscriptionHandler<MsgT>>()->create_subscription_interface(
subscription);
739 }
catch (
const std::exception&
ex) {
741 this->node_logging_->get_logger(),
"Failed to add input '" <<
signal_name <<
"': " <<
ex.what());
745template<
typename DataT>
748 const std::shared_ptr<DataT>& data,
const std::string& default_topic,
bool fixed_topic,
bool publish_on_step) {
751 if (
data ==
nullptr) {
757 this->node_logging_->get_logger(),
759 auto message_pair = make_shared_message_pair(
data, this->node_clock_->get_clock());
766 }
catch (
const std::exception&
ex) {
772inline bool ComponentInterface::remove_signal(
773 const std::string& signal_name, std::map<std::string, std::shared_ptr<T>>& signal_map,
bool skip_check) {
784inline void ComponentInterface::publish_transforms(
785 const std::vector<state_representation::CartesianPose>& transforms,
const std::shared_ptr<T>& tf_broadcaster,
790 this->node_logging_->get_logger(), *
this->node_clock_->get_clock(), 1000,
791 "Failed to send " <<
modifier <<
"transform: No " <<
modifier <<
"TF broadcaster configured.");
803 }
catch (
const std::exception&
ex) {
805 this->node_logging_->get_logger(), *
this->node_clock_->get_clock(), 1000,
806 "Failed to send " <<
modifier <<
"transform: " <<
ex.what());
Friend class to the ComponentInterface to allow test fixtures to access protected and private members...
Base interface class for modulo components to wrap a ROS Node with custom behaviour.
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 set_parameter_value(const std::string &name, const T &value)
Set the value of a parameter.
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.
void add_input(const std::string &signal_name, const std::shared_ptr< DataT > &data, const std::string &default_topic="", bool fixed_topic=false)
Add and configure an input signal of the component.
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.
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.
std::string create_output(modulo_core::communication::PublisherType publisher_type, const std::string &signal_name, const std::shared_ptr< DataT > &data, const std::string &default_topic, bool fixed_topic, bool publish_on_step)
Helper function to parse the signal name and add an unconfigured PublisherInterface to the map of out...
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 signal.
An exception class to notify that a certain pointer is null.
An exception class to notify errors with parameters in modulo classes.
Modulo Core communication module for handling messages on publication and subscription interfaces.
PublisherType
Enum of supported ROS publisher types for the PublisherInterface.
void write_message(geometry_msgs::msg::Accel &message, const state_representation::CartesianState &state, const rclcpp::Time &time)
Convert a CartesianState to a ROS geometry_msgs::msg::Accel.
rclcpp::Parameter write_parameter(const std::shared_ptr< state_representation::ParameterInterface > ¶meter)
Write a ROS Parameter from a ParameterInterface pointer.
Response structure to be returned by component services.