Modulo 5.0.0
Loading...
Searching...
No Matches
ComponentInterface.hpp
1#pragma once
2
3#include <rclcpp/node_interfaces/node_interfaces.hpp>
4
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>
9
10#include <state_representation/parameters/ParameterMap.hpp>
11
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>
19
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>
24
25#include <modulo_utils/parsing.hpp>
26
31namespace modulo_components {
32
40 bool success;
41 std::string message;
42};
43
49
58public:
60
64 virtual ~ComponentInterface();
65
66protected:
71 explicit ComponentInterface(
72 const std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>>& interfaces);
73
78 double get_rate() const;
79
84 template<typename T>
85 T get_period() const;
86
90 virtual void step();
91
95 virtual void on_step_callback();
96
106 void add_parameter(
107 const std::shared_ptr<state_representation::ParameterInterface>& parameter, const std::string& description,
108 bool read_only = false);
109
121 template<typename T>
122 void add_parameter(const std::string& name, const T& value, const std::string& description, bool read_only = false);
123
130 [[nodiscard]] std::shared_ptr<state_representation::ParameterInterface> get_parameter(const std::string& name) const;
131
139 template<typename T>
140 [[nodiscard]] T get_parameter_value(const std::string& name) const;
141
150 template<typename T>
151 void set_parameter_value(const std::string& name, const T& value);
152
163 virtual bool
164 on_validate_parameter_callback(const std::shared_ptr<state_representation::ParameterInterface>& parameter);
165
171 template<typename T>
172 void add_assignment(const std::string& assignment_name);
173
180 template<typename T>
181 void set_assignment(const std::string& assignment_name, const T& assignment_value);
182
191 template<typename T>
192 T get_assignment(const std::string& assignment_name) const;
193
199 void add_predicate(const std::string& predicate_name, bool predicate_value);
200
206 void add_predicate(const std::string& predicate_name, const std::function<bool(void)>& predicate_function);
207
214 [[nodiscard]] bool get_predicate(const std::string& predicate_name) const;
215
223 void set_predicate(const std::string& predicate_name, bool predicate_value);
224
232 void set_predicate(const std::string& predicate_name, const std::function<bool(void)>& predicate_function);
233
239 void add_trigger(const std::string& trigger_name);
240
245 void trigger(const std::string& trigger_name);
246
255 void declare_input(const std::string& signal_name, const std::string& default_topic = "", bool fixed_topic = false);
256
265 void declare_output(const std::string& signal_name, const std::string& default_topic = "", bool fixed_topic = false);
266
275 template<typename DataT>
276 void add_input(
277 const std::string& signal_name, const std::shared_ptr<DataT>& data, const std::string& default_topic = "",
278 bool fixed_topic = false);
279
289 template<typename DataT>
290 void add_input(
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);
293
302 template<typename MsgT>
303 void add_input(
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);
306
319 template<typename DataT>
320 std::string create_output(
321 modulo_core::communication::PublisherType publisher_type, const std::string& signal_name,
322 const std::shared_ptr<DataT>& data, const std::string& default_topic, bool fixed_topic, bool publish_on_step);
323
329 void publish_output(const std::string& signal_name);
330
335 void remove_input(const std::string& signal_name);
336
341 void remove_output(const std::string& signal_name);
342
348 void add_service(const std::string& service_name, const std::function<ComponentServiceResponse(void)>& callback);
349
358 void add_service(
359 const std::string& service_name,
360 const std::function<ComponentServiceResponse(const std::string& string)>& callback);
361
368 void add_periodic_callback(const std::string& name, const std::function<void(void)>& callback);
369
373 void add_tf_broadcaster();
374
379
383 void add_tf_listener();
384
389 void send_transform(const state_representation::CartesianPose& transform);
390
395 void send_transforms(const std::vector<state_representation::CartesianPose>& transforms);
396
401 void send_static_transform(const state_representation::CartesianPose& transform);
402
407 void send_static_transforms(const std::vector<state_representation::CartesianPose>& transforms);
408
419 [[nodiscard]] state_representation::CartesianPose lookup_transform(
420 const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
421 const tf2::Duration& duration);
422
433 [[nodiscard]] state_representation::CartesianPose lookup_transform(
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)));
436
441 [[nodiscard]] rclcpp::QoS get_qos() const;
442
447 void set_qos(const rclcpp::QoS& qos);
448
452 virtual void raise_error();
453
457 void publish_predicates();
458
462 void publish_outputs();
463
468
472 void finalize_interfaces();
473
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_;
476 std::map<std::string, bool> periodic_outputs_;
477
478private:
483 void pre_set_parameters_callback(std::vector<rclcpp::Parameter>& parameters);
484
491 rcl_interfaces::msg::SetParametersResult on_set_parameters_callback(const std::vector<rclcpp::Parameter>& parameters);
492
500 bool validate_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter);
501
507 modulo_interfaces::msg::Predicate get_predicate_message(const std::string& name, bool value) const;
508
518 void declare_signal(
519 const std::string& signal_name, const std::string& type, const std::string& default_topic, bool fixed_topic);
520
529 template<typename T>
530 bool remove_signal(
531 const std::string& signal_name, std::map<std::string, std::shared_ptr<T>>& signal_map, bool skip_check = false);
532
541 std::string validate_service_name(const std::string& service_name, const std::string& type) const;
542
548 void publish_predicate(const std::string& predicate_name, bool value) const;
549
557 template<typename T>
558 void publish_transforms(
559 const std::vector<state_representation::CartesianPose>& transforms, const std::shared_ptr<T>& tf_broadcaster,
560 bool is_static = false);
561
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);
575
576 double rate_;
577 double period_;
578 std::mutex step_mutex_;
579
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_;
585
586 state_representation::ParameterMap assignments_map_;
587 std::shared_ptr<rclcpp::Publisher<modulo_interfaces::msg::Assignment>> assignment_publisher_;
588
589 std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::EmptyTrigger>>>
590 empty_services_;
591 std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::StringTrigger>>>
592 string_services_;
593
594 std::map<std::string, std::function<void(void)>> periodic_callbacks_;
595
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;
604
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_;
610
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);
619};
620
621template<typename T>
623 const std::string& name, const T& value, const std::string& description, bool read_only) {
624 if (name.empty()) {
625 RCLCPP_ERROR(this->node_logging_->get_logger(), "Failed to add parameter: Provide a non empty string as a name.");
626 return;
627 }
628 this->add_parameter(state_representation::make_shared_parameter(name, value), description, read_only);
629}
630
631template<typename T>
632inline T ComponentInterface::get_parameter_value(const std::string& name) const {
633 try {
634 return this->parameter_map_.template get_parameter_value<T>(name);
635 } catch (const state_representation::exceptions::InvalidParameterException& ex) {
637 "Failed to get parameter value of parameter '" + name + "': " + ex.what());
638 }
639}
640
641template<typename T>
642inline void ComponentInterface::set_parameter_value(const std::string& name, const T& value) {
643 try {
644 std::vector<rclcpp::Parameter> parameters{
645 modulo_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))};
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);
653 }
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());
658 }
659}
660
661template<typename DataT>
663 const std::string& signal_name, const std::shared_ptr<DataT>& data, const std::string& default_topic,
664 bool fixed_topic) {
665 this->add_input(signal_name, data, [] {}, default_topic, fixed_topic);
666}
667
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) {
672 using namespace modulo_core::communication;
673 try {
674 if (data == nullptr) {
675 throw modulo_core::exceptions::NullPointerException("Invalid data pointer for input '" + signal_name + "'.");
676 }
677 this->declare_input(signal_name, default_topic, fixed_topic);
678 std::string parsed_signal_name = modulo_utils::parsing::parse_topic_name(signal_name);
679 auto topic_name = this->get_parameter_value<std::string>(parsed_signal_name + "_topic");
680 RCLCPP_DEBUG_STREAM(
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);
693 break;
694 }
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);
702 break;
703 }
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);
711 break;
712 }
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);
720 break;
721 }
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);
729 break;
730 }
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);
738 break;
739 }
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);
748 }
749 break;
750 }
751 }
752 this->inputs_.insert_or_assign(parsed_signal_name, subscription_interface);
753 } catch (const std::exception& ex) {
754 RCLCPP_ERROR_STREAM(
755 this->node_logging_->get_logger(), "Failed to add input '" << signal_name << "': " << ex.what());
756 }
757}
758
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) {
763 using namespace modulo_core::communication;
764 try {
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);
767 auto topic_name = this->get_parameter_value<std::string>(parsed_signal_name + "_topic");
768 RCLCPP_DEBUG_STREAM(
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) {
774 try {
775 callback(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());
780 }
781 });
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) {
786 RCLCPP_ERROR_STREAM(
787 this->node_logging_->get_logger(), "Failed to add input '" << signal_name << "': " << ex.what());
788 }
789}
790
791template<typename DataT>
793 modulo_core::communication::PublisherType publisher_type, const std::string& signal_name,
794 const std::shared_ptr<DataT>& data, const std::string& default_topic, bool fixed_topic, bool publish_on_step) {
795 using namespace modulo_core::communication;
796 try {
797 if (data == nullptr) {
798 throw modulo_core::exceptions::NullPointerException("Invalid data pointer for output '" + signal_name + "'.");
799 }
800 this->declare_output(signal_name, default_topic, fixed_topic);
801 auto parsed_signal_name = modulo_utils::parsing::parse_topic_name(signal_name);
802 RCLCPP_DEBUG_STREAM(
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());
806 this->outputs_.insert_or_assign(
807 parsed_signal_name, std::make_shared<PublisherInterface>(publisher_type, message_pair));
808 this->periodic_outputs_.insert_or_assign(parsed_signal_name, publish_on_step);
809 return parsed_signal_name;
811 throw;
812 } catch (const std::exception& ex) {
814 }
815}
816
817template<typename T>
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()) {
821 return false;
822 } else {
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);
826 }
827}
828
829template<typename T>
830inline void ComponentInterface::publish_transforms(
831 const std::vector<state_representation::CartesianPose>& transforms, const std::shared_ptr<T>& tf_broadcaster,
832 bool is_static) {
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.");
838 return;
839 }
840 try {
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;
845 modulo_core::translators::write_message(transform_message, tf, this->node_clock_->get_clock()->now());
846 transform_messages.emplace_back(transform_message);
847 }
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());
853 }
854}
855
856template<typename T>
857inline void ComponentInterface::add_assignment(const std::string& assignment_name) {
858 std::string parsed_name = modulo_utils::parsing::parse_topic_name(assignment_name);
859 if (parsed_name.empty()) {
860 RCLCPP_ERROR_STREAM(
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_]).");
864 return;
865 }
866 if (assignment_name != parsed_name) {
867 RCLCPP_WARN_STREAM(
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.");
871 }
872 try {
873 this->assignments_map_.get_parameter(parsed_name);
874 RCLCPP_WARN_STREAM(
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 << "'.");
878 }
879 try {
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());
885 }
886}
887
888template<typename T>
889void ComponentInterface::set_assignment(const std::string& assignment_name, const T& assignment_value) {
890 modulo_interfaces::msg::Assignment message;
891 std::shared_ptr<state_representation::ParameterInterface> assignment;
892 try {
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.");
898 return;
899 }
900 try {
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.");
906 return;
907 }
908 message.node = this->node_base_->get_fully_qualified_name();
909 message.assignment = modulo_core::translators::write_parameter(assignment).to_parameter_msg();
910 this->assignment_publisher_->publish(message);
911}
912
913template<typename T>
914T ComponentInterface::get_assignment(const std::string& assignment_name) const {
915 std::shared_ptr<state_representation::ParameterInterface> assignment;
916 try {
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.");
921 }
922 try {
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 + "'.");
928 }
929}
930}// namespace modulo_components
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 > &parameter)
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 > &parameter, 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 components.
Definition Component.hpp:9
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 > &parameter)
Write a ROS Parameter from a ParameterInterface pointer.
Response structure to be returned by component services.