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/assignment.hpp>
21#include <modulo_interfaces/msg/predicate_collection.hpp>
22#include <modulo_interfaces/srv/empty_trigger.hpp>
23#include <modulo_interfaces/srv/string_trigger.hpp>
25#include <modulo_utils/parsing.hpp>
72 const std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>>& interfaces);
107 const std::shared_ptr<state_representation::ParameterInterface>& parameter,
const std::string& description,
108 bool read_only =
false);
122 void add_parameter(
const std::string& name,
const T& value,
const std::string& description,
bool read_only =
false);
130 [[nodiscard]] std::shared_ptr<state_representation::ParameterInterface>
get_parameter(
const std::string& name)
const;
181 void set_assignment(
const std::string& assignment_name,
const T& assignment_value);
199 void add_predicate(
const std::string& predicate_name,
bool predicate_value);
206 void add_predicate(
const std::string& predicate_name,
const std::function<
bool(
void)>& predicate_function);
214 [[nodiscard]]
bool get_predicate(
const std::string& predicate_name)
const;
223 void set_predicate(
const std::string& predicate_name,
bool predicate_value);
232 void set_predicate(
const std::string& predicate_name,
const std::function<
bool(
void)>& predicate_function);
245 void trigger(
const std::string& trigger_name);
255 void declare_input(
const std::string& signal_name,
const std::string& default_topic =
"",
bool fixed_topic =
false);
265 void declare_output(
const std::string& signal_name,
const std::string& default_topic =
"",
bool fixed_topic =
false);
275 template<
typename DataT>
277 const std::string& signal_name,
const std::shared_ptr<DataT>& data,
const std::string& default_topic =
"",
278 bool fixed_topic =
false);
289 template<
typename DataT>
291 const std::string& signal_name,
const std::shared_ptr<DataT>& data,
const std::function<
void()>& callback,
292 const std::string& default_topic =
"",
bool fixed_topic =
false);
302 template<
typename MsgT>
304 const std::string& signal_name,
const std::function<
void(
const std::shared_ptr<MsgT>)>& callback,
305 const std::string& default_topic =
"",
bool fixed_topic =
false);
319 template<
typename DataT>
322 const std::shared_ptr<DataT>& data,
const std::string& default_topic,
bool fixed_topic,
bool publish_on_step);
359 const std::string& service_name,
389 void send_transform(
const state_representation::CartesianPose& transform);
395 void send_transforms(
const std::vector<state_representation::CartesianPose>& transforms);
420 const std::string& frame,
const std::string& reference_frame,
const tf2::TimePoint& time_point,
421 const tf2::Duration& duration);
434 const std::string& frame,
const std::string& reference_frame =
"world",
double validity_period = -1.0,
435 const tf2::Duration& duration = tf2::Duration(std::chrono::microseconds(10)));
441 [[nodiscard]] rclcpp::QoS
get_qos()
const;
447 void set_qos(
const rclcpp::QoS& qos);
474 std::map<std::string, std::shared_ptr<modulo_core::communication::SubscriptionInterface>>
inputs_;
475 std::map<std::string, std::shared_ptr<modulo_core::communication::PublisherInterface>>
outputs_;
483 void pre_set_parameters_callback(std::vector<rclcpp::Parameter>& parameters);
491 rcl_interfaces::msg::SetParametersResult on_set_parameters_callback(
const std::vector<rclcpp::Parameter>& parameters);
500 bool validate_parameter(
const std::shared_ptr<state_representation::ParameterInterface>& parameter);
507 modulo_interfaces::msg::Predicate get_predicate_message(
const std::string& name,
bool value)
const;
519 const std::string& signal_name,
const std::string& type,
const std::string& default_topic,
bool fixed_topic);
531 const std::string& signal_name, std::map<std::string, std::shared_ptr<T>>& signal_map,
bool skip_check =
false);
541 std::string validate_service_name(
const std::string& service_name,
const std::string& type)
const;
548 void publish_predicate(
const std::string& predicate_name,
bool value)
const;
558 void publish_transforms(
559 const std::vector<state_representation::CartesianPose>& transforms,
const std::shared_ptr<T>& tf_broadcaster,
560 bool is_static =
false);
572 [[nodiscard]] geometry_msgs::msg::TransformStamped lookup_ros_transform(
573 const std::string& frame,
const std::string& reference_frame,
const tf2::TimePoint& time_point,
574 const tf2::Duration& duration);
578 std::mutex step_mutex_;
580 std::map<std::string, modulo_core::Predicate> predicates_;
581 std::shared_ptr<rclcpp::Publisher<modulo_interfaces::msg::PredicateCollection>>
582 predicate_publisher_;
583 modulo_interfaces::msg::PredicateCollection predicate_message_;
584 std::vector<std::string> triggers_;
586 state_representation::ParameterMap assignments_map_;
587 std::shared_ptr<rclcpp::Publisher<modulo_interfaces::msg::Assignment>> assignment_publisher_;
589 std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::EmptyTrigger>>>
591 std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::StringTrigger>>>
594 std::map<std::string, std::function<void(
void)>> periodic_callbacks_;
596 state_representation::ParameterMap parameter_map_;
597 std::unordered_map<std::string, bool> read_only_parameters_;
598 std::shared_ptr<rclcpp::node_interfaces::PreSetParametersCallbackHandle>
599 pre_set_parameter_cb_handle_;
600 std::shared_ptr<rclcpp::node_interfaces::OnSetParametersCallbackHandle>
601 on_set_parameter_cb_handle_;
602 rcl_interfaces::msg::SetParametersResult set_parameters_result_;
603 bool pre_set_parameter_callback_called_ =
false;
605 std::shared_ptr<rclcpp::TimerBase> step_timer_;
606 std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
607 std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
608 std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
609 std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
611 std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;
612 std::shared_ptr<rclcpp::node_interfaces::NodeClockInterface> node_clock_;
613 std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> node_logging_;
614 std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> node_parameters_;
615 std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services_;
616 std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface> node_timers_;
617 std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> node_topics_;
618 rclcpp::QoS qos_ = rclcpp::QoS(10);
623 const std::string& name,
const T& value,
const std::string& description,
bool read_only) {
625 RCLCPP_ERROR(this->node_logging_->get_logger(),
"Failed to add parameter: Provide a non empty string as a name.");
628 this->
add_parameter(state_representation::make_shared_parameter(name, value), description, read_only);
635 }
catch (
const state_representation::exceptions::InvalidParameterException& ex) {
637 "Failed to get parameter value of parameter '" + name +
"': " + ex.what());
644 std::vector<rclcpp::Parameter> parameters{
646 this->pre_set_parameters_callback(parameters);
647 this->pre_set_parameter_callback_called_ =
true;
648 auto result = this->node_parameters_->set_parameters(parameters).at(0);
649 if (!result.successful) {
650 RCLCPP_ERROR_STREAM_THROTTLE(
651 this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
652 "Failed to set parameter value of parameter '" << name <<
"': " << result.reason);
654 }
catch (
const std::exception& ex) {
655 RCLCPP_ERROR_STREAM_THROTTLE(
656 this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
657 "Failed to set parameter value of parameter '" << name <<
"': " << ex.what());
661template<
typename DataT>
663 const std::string& signal_name,
const std::shared_ptr<DataT>& data,
const std::string& default_topic,
665 this->
add_input(signal_name, data, [] {}, default_topic, fixed_topic);
668template<
typename DataT>
670 const std::string& signal_name,
const std::shared_ptr<DataT>& data,
const std::function<
void()>& user_callback,
671 const std::string& default_topic,
bool fixed_topic) {
674 if (data ==
nullptr) {
677 this->
declare_input(signal_name, default_topic, fixed_topic);
678 std::string parsed_signal_name = modulo_utils::parsing::parse_topic_name(signal_name);
681 this->node_logging_->get_logger(),
682 "Adding input '" << parsed_signal_name <<
"' with topic name '" << topic_name <<
"'.");
683 auto message_pair = make_shared_message_pair(data, this->node_clock_->get_clock());
684 std::shared_ptr<SubscriptionInterface> subscription_interface;
685 switch (message_pair->get_type()) {
686 case MessageType::BOOL: {
687 auto subscription_handler =
688 std::make_shared<SubscriptionHandler<std_msgs::msg::Bool>>(message_pair, this->node_logging_->get_logger());
689 auto subscription = rclcpp::create_subscription<std_msgs::msg::Bool>(
690 this->node_parameters_, this->node_topics_, topic_name, this->qos_,
691 subscription_handler->get_callback(user_callback));
692 subscription_interface = subscription_handler->create_subscription_interface(subscription);
695 case MessageType::FLOAT64: {
696 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::Float64>>(
697 message_pair, this->node_logging_->get_logger());
698 auto subscription = rclcpp::create_subscription<std_msgs::msg::Float64>(
699 this->node_parameters_, this->node_topics_, topic_name, this->qos_,
700 subscription_handler->get_callback(user_callback));
701 subscription_interface = subscription_handler->create_subscription_interface(subscription);
704 case MessageType::FLOAT64_MULTI_ARRAY: {
705 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::Float64MultiArray>>(
706 message_pair, this->node_logging_->get_logger());
707 auto subscription = rclcpp::create_subscription<std_msgs::msg::Float64MultiArray>(
708 this->node_parameters_, this->node_topics_, topic_name, this->qos_,
709 subscription_handler->get_callback(user_callback));
710 subscription_interface = subscription_handler->create_subscription_interface(subscription);
713 case MessageType::INT32: {
714 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::Int32>>(
715 message_pair, this->node_logging_->get_logger());
716 auto subscription = rclcpp::create_subscription<std_msgs::msg::Int32>(
717 this->node_parameters_, this->node_topics_, topic_name, this->qos_,
718 subscription_handler->get_callback(user_callback));
719 subscription_interface = subscription_handler->create_subscription_interface(subscription);
722 case MessageType::STRING: {
723 auto subscription_handler = std::make_shared<SubscriptionHandler<std_msgs::msg::String>>(
724 message_pair, this->node_logging_->get_logger());
725 auto subscription = rclcpp::create_subscription<std_msgs::msg::String>(
726 this->node_parameters_, this->node_topics_, topic_name, this->qos_,
727 subscription_handler->get_callback(user_callback));
728 subscription_interface = subscription_handler->create_subscription_interface(subscription);
731 case MessageType::ENCODED_STATE: {
732 auto subscription_handler = std::make_shared<SubscriptionHandler<modulo_core::EncodedState>>(
733 message_pair, this->node_logging_->get_logger());
734 auto subscription = rclcpp::create_subscription<modulo_core::EncodedState>(
735 this->node_parameters_, this->node_topics_, topic_name, this->qos_,
736 subscription_handler->get_callback(user_callback));
737 subscription_interface = subscription_handler->create_subscription_interface(subscription);
740 case MessageType::CUSTOM_MESSAGE: {
742 auto subscription_handler =
743 std::make_shared<SubscriptionHandler<DataT>>(message_pair, this->node_logging_->get_logger());
744 auto subscription = rclcpp::create_subscription<DataT>(
745 this->node_parameters_, this->node_topics_, topic_name, this->qos_,
746 subscription_handler->get_callback(user_callback));
747 subscription_interface = subscription_handler->create_subscription_interface(subscription);
752 this->
inputs_.insert_or_assign(parsed_signal_name, subscription_interface);
753 }
catch (
const std::exception& ex) {
755 this->node_logging_->get_logger(),
"Failed to add input '" << signal_name <<
"': " << ex.what());
759template<
typename MsgT>
761 const std::string& signal_name,
const std::function<
void(
const std::shared_ptr<MsgT>)>& callback,
762 const std::string& default_topic,
bool fixed_topic) {
765 std::string parsed_signal_name = modulo_utils::parsing::parse_topic_name(signal_name);
766 this->
declare_input(parsed_signal_name, default_topic, fixed_topic);
769 this->node_logging_->get_logger(),
770 "Adding input '" << parsed_signal_name <<
"' with topic name '" << topic_name <<
"'.");
771 auto subscription = rclcpp::create_subscription<MsgT>(
772 this->node_parameters_, this->node_topics_, topic_name, this->qos_,
773 [
this, signal_name, callback](
const std::shared_ptr<MsgT> message) {
776 }
catch (
const std::exception& ex) {
777 RCLCPP_WARN_STREAM_THROTTLE(
778 this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
779 "Unhandled exception in callback of input '" << signal_name <<
"': " << ex.what());
782 auto subscription_interface =
783 std::make_shared<SubscriptionHandler<MsgT>>()->create_subscription_interface(subscription);
784 this->
inputs_.insert_or_assign(parsed_signal_name, subscription_interface);
785 }
catch (
const std::exception& ex) {
787 this->node_logging_->get_logger(),
"Failed to add input '" << signal_name <<
"': " << ex.what());
791template<
typename DataT>
794 const std::shared_ptr<DataT>& data,
const std::string& default_topic,
bool fixed_topic,
bool publish_on_step) {
797 if (data ==
nullptr) {
801 auto parsed_signal_name = modulo_utils::parsing::parse_topic_name(signal_name);
803 this->node_logging_->get_logger(),
804 "Creating output '" << parsed_signal_name <<
"' (provided signal name was '" << signal_name <<
"').");
805 auto message_pair = make_shared_message_pair(data, this->node_clock_->get_clock());
807 parsed_signal_name, std::make_shared<PublisherInterface>(publisher_type, message_pair));
809 return parsed_signal_name;
812 }
catch (
const std::exception& ex) {
818inline bool ComponentInterface::remove_signal(
819 const std::string& signal_name, std::map<std::string, std::shared_ptr<T>>& signal_map,
bool skip_check) {
820 if (!skip_check && signal_map.find(signal_name) == signal_map.cend()) {
823 RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(),
"Removing signal '" << signal_name <<
"'.");
824 signal_map.at(signal_name).reset();
825 return signal_map.erase(signal_name);
830inline void ComponentInterface::publish_transforms(
831 const std::vector<state_representation::CartesianPose>& transforms,
const std::shared_ptr<T>& tf_broadcaster,
833 std::string modifier = is_static ?
"static " :
"";
834 if (tf_broadcaster ==
nullptr) {
835 RCLCPP_ERROR_STREAM_THROTTLE(
836 this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
837 "Failed to send " << modifier <<
"transform: No " << modifier <<
"TF broadcaster configured.");
841 std::vector<geometry_msgs::msg::TransformStamped> transform_messages;
842 transform_messages.reserve(transforms.size());
843 for (
const auto& tf : transforms) {
844 geometry_msgs::msg::TransformStamped transform_message;
846 transform_messages.emplace_back(transform_message);
848 tf_broadcaster->sendTransform(transform_messages);
849 }
catch (
const std::exception& ex) {
850 RCLCPP_ERROR_STREAM_THROTTLE(
851 this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
852 "Failed to send " << modifier <<
"transform: " << ex.what());
858 std::string parsed_name = modulo_utils::parsing::parse_topic_name(assignment_name);
859 if (parsed_name.empty()) {
861 this->node_logging_->get_logger(),
862 "The parsed name for assignment '" + assignment_name
863 +
"' is empty. Provide a string with valid characters for the assignment name ([a-z0-9_]).");
866 if (assignment_name != parsed_name) {
868 this->node_logging_->get_logger(),
869 "The parsed name for assignment '" + assignment_name +
"' is '" + parsed_name
870 +
"'. Use the parsed name to refer to this assignment.");
873 this->assignments_map_.get_parameter(parsed_name);
875 this->node_logging_->get_logger(),
"Assignment with name '" + parsed_name +
"' already exists, overwriting.");
876 }
catch (
const state_representation::exceptions::InvalidParameterException& ex) {
877 RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(),
"Adding assignment '" << parsed_name <<
"'.");
880 assignments_map_.set_parameter(state_representation::make_shared_parameter<T>(parsed_name));
881 }
catch (
const std::exception& ex) {
882 RCLCPP_ERROR_STREAM_THROTTLE(
883 this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
884 "Failed to add assignment '" << parsed_name <<
"': " << ex.what());
890 modulo_interfaces::msg::Assignment message;
891 std::shared_ptr<state_representation::ParameterInterface> assignment;
893 assignment = this->assignments_map_.get_parameter(assignment_name);
894 }
catch (
const state_representation::exceptions::InvalidParameterException&) {
895 RCLCPP_ERROR_STREAM_THROTTLE(
896 this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
897 "Failed to set assignment '" << assignment_name <<
"': Assignment does not exist.");
901 assignment->set_parameter_value<T>(assignment_value);
902 }
catch (
const state_representation::exceptions::InvalidParameterCastException&) {
903 RCLCPP_ERROR_STREAM_THROTTLE(
904 this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
905 "Failed to set assignment '" << assignment_name <<
"': Incompatible value type.");
908 message.node = this->node_base_->get_fully_qualified_name();
910 this->assignment_publisher_->publish(message);
915 std::shared_ptr<state_representation::ParameterInterface> assignment;
917 assignment = this->assignments_map_.get_parameter(assignment_name);
918 }
catch (
const state_representation::exceptions::InvalidParameterException&) {
920 "Failed to get value of assignment '" + assignment_name +
"': Assignment does not exist.");
923 return assignment->get_parameter_value<T>();
924 }
catch (
const state_representation::exceptions::InvalidParameterCastException&) {
925 auto expected_type = state_representation::get_parameter_type_name(assignment->get_parameter_type());
927 "Incompatible type for assignment '" + assignment_name +
"' defined with type '" + expected_type +
"'.");
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.
void finalize_interfaces()
Finalize all interfaces.
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_assignment(const std::string &assignment_name)
Add an assignment to the map of assignments.
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()
Notify an error in the component.
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.
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 set_assignment(const std::string &assignment_name, const T &assignment_value)
Set the value of an assignment.
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.
T get_assignment(const std::string &assignment_name) const
Get the value of an assignment.
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 errors when getting the value of an assignment.
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.