From 3e6664ff409e3bc791afe51968ab6280383ea0e3 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Tue, 24 Oct 2023 17:30:43 +0200 Subject: [PATCH 1/5] refactor: change component interface to use node interfaces (#33) Remove template class NodeT and change ComponentInterface constructor arguments to get all the node interfaces. Pass the publisher type as argument to create_output. --- CHANGELOG.md | 4 + source/modulo_components/CMakeLists.txt | 1 + .../modulo_components/ComponentInterface.hpp | 429 ++++++++---------- .../src/ComponentInterface.cpp | 35 ++ .../component_public_interfaces.hpp | 97 ++-- .../test/cpp/test_component_interface.cpp | 87 ++-- ...t_component_interface_empty_parameters.cpp | 84 +--- .../test_component_interface_parameters.cpp | 11 +- 8 files changed, 342 insertions(+), 406 deletions(-) create mode 100644 source/modulo_components/src/ComponentInterface.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 817781899..ca017e473 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,6 +10,10 @@ Release Versions: - [2.1.1](#211) - [2.1.0](#210) +## Upcoming changes (in development) + +- Refactor ComponentInterface to use node interfaces (#33) + ## 3.2.0 ### October 23, 2023 diff --git a/source/modulo_components/CMakeLists.txt b/source/modulo_components/CMakeLists.txt index 1295495df..8a1dfd1e6 100644 --- a/source/modulo_components/CMakeLists.txt +++ b/source/modulo_components/CMakeLists.txt @@ -24,6 +24,7 @@ ament_auto_find_build_dependencies() include_directories(include) ament_auto_add_library(${PROJECT_NAME} SHARED + ${PROJECT_SOURCE_DIR}/src/ComponentInterface.cpp ${PROJECT_SOURCE_DIR}/src/Component.cpp ${PROJECT_SOURCE_DIR}/src/LifecycleComponent.cpp) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 2991f6e27..d13254854 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -1,10 +1,6 @@ #pragma once -// TODO sort includes -#include -#include -#include -#include +#include #include #include @@ -14,9 +10,7 @@ #include #include -#include -#include #include #include #include @@ -56,9 +50,7 @@ struct ComponentServiceResponse { /** * @class ComponentInterfacePublicInterface * @brief Friend class to the ComponentInterface to allow test fixtures to access protected and private members. - * @tparam NodeT The rclcpp Node type */ -template class ComponentInterfacePublicInterface; /** @@ -67,33 +59,25 @@ class ComponentInterfacePublicInterface; * @details This class is not intended for direct inheritance and usage by end-users. Instead, it defines the common * interfaces for the derived classes modulo_components::Component and modulo_components::LifecycleComponent. * @see Component, LifecycleComponent - * @tparam NodeT The rclcpp Node type */ -template -class ComponentInterface : public NodeT { +class ComponentInterface { public: - friend class ComponentInterfacePublicInterface; - friend class ComponentInterfacePublicInterface; - - /** - * @brief Constructor from node options. - * @param node_options Node options as used in ROS2 Node / LifecycleNode - * @param publisher_type The type of publisher that also indicates if the component is lifecycle or not - * @param fallback_name The name of the component if it was not provided through the node options - */ - explicit ComponentInterface( - const rclcpp::NodeOptions& node_options, modulo_core::communication::PublisherType publisher_type, - const std::string& fallback_name = "ComponentInterface" - ); + friend class ComponentInterfacePublicInterface; /** * @brief Virtual default destructor. */ virtual ~ComponentInterface() = default; - // TODO hide ROS methods - protected: + /** + * @brief Constructor with all node interfaces + * @param interfaces Shared pointer to all the node interfaces of parent class + */ + explicit ComponentInterface( + const std::shared_ptr>& interfaces + ); + /** * @brief Step function that is called periodically. */ @@ -359,8 +343,8 @@ class ComponentInterface : public NodeT { */ template std::string create_output( - const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic, - bool fixed_topic, bool publish_on_step + modulo_core::communication::PublisherType publisher_type, const std::string& signal_name, + const std::shared_ptr& data, const std::string& default_topic, bool fixed_topic, bool publish_on_step ); /** @@ -561,12 +545,9 @@ class ComponentInterface : public NodeT { const tf2::Duration& duration ); - modulo_core::communication::PublisherType - publisher_type_; ///< Type of the output publishers (one of PUBLISHER, LIFECYCLE_PUBLISHER) - std::map predicates_; ///< Map of predicates std::shared_ptr> - predicate_publisher_; ///< Predicate publisher + predicate_publisher_; ///< Predicate publisher std::map triggers_; ///< Map of triggers std::map>> @@ -580,59 +561,41 @@ class ComponentInterface : public NodeT { std::shared_ptr parameter_cb_handle_; ///< ROS callback function handle for setting parameters + std::shared_ptr cb_group_; std::shared_ptr step_timer_; ///< Timer for the step function std::shared_ptr tf_buffer_; ///< TF buffer std::shared_ptr tf_listener_; ///< TF listener std::shared_ptr tf_broadcaster_; ///< TF broadcaster std::shared_ptr static_tf_broadcaster_; ///< TF static broadcaster + std::shared_ptr node_base_; + std::shared_ptr node_clock_; + std::shared_ptr node_logging_; + std::shared_ptr node_parameters_; + std::shared_ptr node_services_; + std::shared_ptr node_timers_; + std::shared_ptr node_topics_; + bool set_parameter_callback_called_ = false; ///< Flag to indicate if on_set_parameter_callback was called }; -template -ComponentInterface::ComponentInterface( - const rclcpp::NodeOptions& options, modulo_core::communication::PublisherType publisher_type, - const std::string& fallback_name) - : NodeT(utilities::parse_node_name(options, fallback_name), utilities::modify_parameter_overrides(options)), - publisher_type_(publisher_type) { - // register the parameter change callback handler - parameter_cb_handle_ = NodeT::add_on_set_parameters_callback( - [this](const std::vector& 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->predicate_publisher_ = - rclcpp::create_publisher(*this, "/predicates", this->qos_); - this->add_predicate("in_error_state", false); - - this->step_timer_ = this->create_wall_timer( - std::chrono::nanoseconds(static_cast(this->get_parameter_value("period") * 1e9)), - [this] { this->step(); }); -} - -template -inline void ComponentInterface::step() {} +inline void ComponentInterface::step() {} -template -inline void ComponentInterface::on_step_callback() {} +inline void ComponentInterface::on_step_callback() {} -template template -inline void ComponentInterface::add_parameter( +inline void ComponentInterface::add_parameter( const std::string& name, const T& value, const std::string& description, bool read_only ) { if (name.empty()) { - RCLCPP_ERROR(this->get_logger(), "Failed to add parameter: Provide a non empty string as a name."); + RCLCPP_ERROR(this->node_logging_->get_logger(), "Failed to add parameter: Provide a non empty string as a name."); return; } this->add_parameter(state_representation::make_shared_parameter(name, value), description, read_only); } -template template -inline T ComponentInterface::get_parameter_value(const std::string& name) const { +inline T ComponentInterface::get_parameter_value(const std::string& name) const { try { return this->parameter_map_.template get_parameter_value(name); } catch (const state_representation::exceptions::InvalidParameterException& ex) { @@ -641,8 +604,7 @@ inline T ComponentInterface::get_parameter_value(const std::string& name) } } -template -inline void ComponentInterface::add_parameter( +inline void ComponentInterface::add_parameter( const std::shared_ptr& parameter, const std::string& description, bool read_only ) { @@ -653,8 +615,8 @@ inline void ComponentInterface::add_parameter( } catch (const modulo_core::exceptions::ParameterTranslationException& ex) { throw exceptions::ComponentParameterException("Failed to add parameter: " + std::string(ex.what())); } - if (!NodeT::has_parameter(parameter->get_name())) { - RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding parameter '" << parameter->get_name() << "'."); + if (!this->node_parameters_->has_parameter(parameter->get_name())) { + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding parameter '" << parameter->get_name() << "'."); this->parameter_map_.set_parameter(parameter); try { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -663,14 +625,15 @@ inline void ComponentInterface::add_parameter( if (parameter->is_empty()) { descriptor.dynamic_typing = true; descriptor.type = modulo_core::translators::get_ros_parameter_type(parameter->get_parameter_type()); - NodeT::declare_parameter(parameter->get_name(), rclcpp::ParameterValue{}, descriptor); + this->node_parameters_->declare_parameter(parameter->get_name(), rclcpp::ParameterValue{}, descriptor); } else { - NodeT::declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor); + this->node_parameters_->declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor); } if (!this->set_parameter_callback_called_) { - auto result = this->on_set_parameters_callback({NodeT::get_parameters({parameter->get_name()})}); + auto result = + this->on_set_parameters_callback({this->node_parameters_->get_parameters({parameter->get_name()})}); if (!result.successful) { - NodeT::undeclare_parameter(parameter->get_name()); + this->node_parameters_->undeclare_parameter(parameter->get_name()); throw exceptions::ComponentParameterException(result.reason); } } @@ -679,13 +642,13 @@ inline void ComponentInterface::add_parameter( throw exceptions::ComponentParameterException("Failed to add parameter: " + std::string(ex.what())); } } else { - RCLCPP_DEBUG_STREAM(this->get_logger(), "Parameter '" << parameter->get_name() << "' already exists."); + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), + "Parameter '" << parameter->get_name() << "' already exists."); } } -template inline std::shared_ptr -ComponentInterface::get_parameter(const std::string& name) const { +ComponentInterface::get_parameter(const std::string& name) const { try { return this->parameter_map_.get_parameter(name); } catch (const state_representation::exceptions::InvalidParameterException& ex) { @@ -693,53 +656,53 @@ ComponentInterface::get_parameter(const std::string& name) const { } } -template template -inline void ComponentInterface::set_parameter_value(const std::string& name, const T& value) { +inline void ComponentInterface::set_parameter_value(const std::string& name, const T& value) { try { - rcl_interfaces::msg::SetParametersResult result = NodeT::set_parameter( - modulo_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))); + rcl_interfaces::msg::SetParametersResult result = this->node_parameters_->set_parameters( + { + modulo_core::translators::write_parameter(state_representation::make_shared_parameter(name, value)) + }).at(0); if (!result.successful) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, "Failed to set parameter value of parameter '" << name << "': " << result.reason); } } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, "Failed to set parameter value of parameter '" << name << "': " << ex.what()); } } -template -inline bool ComponentInterface::validate_parameter( +inline bool ComponentInterface::validate_parameter( const std::shared_ptr& parameter ) { if (parameter->get_name() == "rate") { auto value = parameter->get_parameter_value(); if (value <= 0 || !std::isfinite(value)) { - RCLCPP_ERROR(this->get_logger(), "Value for parameter 'rate' has to be a positive finite number."); + 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(); if (value <= 0.0 || !std::isfinite(value)) { - RCLCPP_ERROR(this->get_logger(), "Value for parameter 'period' has to be a positive finite number."); + 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); } -template -inline bool ComponentInterface::on_validate_parameter_callback( +inline bool ComponentInterface::on_validate_parameter_callback( const std::shared_ptr& ) { return true; } -template inline rcl_interfaces::msg::SetParametersResult -ComponentInterface::on_set_parameters_callback(const std::vector& parameters) { +ComponentInterface::on_set_parameters_callback(const std::vector& parameters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; for (const auto& ros_parameter : parameters) { @@ -768,40 +731,37 @@ ComponentInterface::on_set_parameters_callback(const std::vector -inline void ComponentInterface::add_predicate(const std::string& name, bool predicate) { +inline void ComponentInterface::add_predicate(const std::string& name, bool predicate) { this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); } -template -inline void ComponentInterface::add_predicate( +inline void ComponentInterface::add_predicate( const std::string& name, const std::function& predicate ) { this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); } -template -inline void ComponentInterface::add_variant_predicate( +inline void ComponentInterface::add_variant_predicate( const std::string& name, const utilities::PredicateVariant& predicate ) { if (name.empty()) { - RCLCPP_ERROR(this->get_logger(), "Failed to add predicate: Provide a non empty string as a name."); + RCLCPP_ERROR(this->node_logging_->get_logger(), "Failed to add predicate: Provide a non empty string as a name."); return; } if (this->predicates_.find(name) != this->predicates_.end()) { - RCLCPP_WARN_STREAM(this->get_logger(), "Predicate with name '" << name << "' already exists, overwriting."); + RCLCPP_WARN_STREAM(this->node_logging_->get_logger(), + "Predicate with name '" << name << "' already exists, overwriting."); } else { - RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding predicate '" << name << "'."); + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding predicate '" << name << "'."); } this->predicates_.insert_or_assign(name, predicate); } -template -inline bool ComponentInterface::get_predicate(const std::string& predicate_name) { +inline bool ComponentInterface::get_predicate(const std::string& predicate_name) { auto predicate_iterator = this->predicates_.find(predicate_name); // if there is no predicate with that name simply return false with an error message if (predicate_iterator == this->predicates_.end()) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, "Failed to get predicate '" << predicate_name << "': Predicate does not exists, returning false."); return false; @@ -817,23 +777,22 @@ inline bool ComponentInterface::get_predicate(const std::string& predicat try { value = (callback_function)(); } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, "Failed to evaluate callback of predicate '" << predicate_name << "', returning false: " << ex.what()); } return value; } -template -inline void ComponentInterface::add_trigger(const std::string& trigger_name) { +inline void ComponentInterface::add_trigger(const std::string& trigger_name) { if (trigger_name.empty()) { - RCLCPP_ERROR(this->get_logger(), "Failed to add trigger: Provide a non empty string as a name."); + RCLCPP_ERROR(this->node_logging_->get_logger(), "Failed to add trigger: Provide a non empty string as a name."); return; } if (this->triggers_.find(trigger_name) != this->triggers_.end() || this->predicates_.find(trigger_name) != this->predicates_.end()) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add trigger: there is already a trigger or " - "predicate with name '" << trigger_name << "'."); + RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), "Failed to add trigger: there is already a trigger or " + "predicate with name '" << trigger_name << "'."); return; } this->triggers_.insert_or_assign(trigger_name, false); @@ -845,24 +804,22 @@ inline void ComponentInterface::add_trigger(const std::string& trigger_na }); } -template -inline void ComponentInterface::trigger(const std::string& trigger_name) { +inline void ComponentInterface::trigger(const std::string& trigger_name) { if (this->triggers_.find(trigger_name) == this->triggers_.end()) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to trigger: could not find trigger" - " with name '" << trigger_name << "'."); + RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), "Failed to trigger: could not find trigger" + " with name '" << trigger_name << "'."); return; } this->triggers_.at(trigger_name) = true; publish_predicate(trigger_name); } -template -inline void ComponentInterface::set_variant_predicate( +inline void ComponentInterface::set_variant_predicate( const std::string& name, const utilities::PredicateVariant& predicate ) { auto predicate_iterator = this->predicates_.find(name); if (predicate_iterator == this->predicates_.end()) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, "Failed to set predicate '" << name << "': Predicate does not exist."); return; } @@ -870,56 +827,50 @@ inline void ComponentInterface::set_variant_predicate( this->publish_predicate(name); } -template -inline void ComponentInterface::set_predicate(const std::string& name, bool predicate) { +inline void ComponentInterface::set_predicate(const std::string& name, bool predicate) { this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); } -template -inline void ComponentInterface::set_predicate( +inline void ComponentInterface::set_predicate( const std::string& name, const std::function& predicate ) { this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); } -template template -inline bool ComponentInterface::remove_signal( +inline bool ComponentInterface::remove_signal( const std::string& signal_name, std::map>& signal_map, bool skip_check ) { if (!skip_check && signal_map.find(signal_name) == signal_map.cend()) { return false; } else { - RCLCPP_DEBUG_STREAM(this->get_logger(), "Removing signal '" << signal_name << "'."); + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Removing signal '" << signal_name << "'."); signal_map.at(signal_name).reset(); return signal_map.erase(signal_name); } } -template -inline void ComponentInterface::remove_input(const std::string& signal_name) { +inline void ComponentInterface::remove_input(const std::string& signal_name) { if (!this->template remove_signal(signal_name, this->inputs_)) { auto parsed_signal_name = utilities::parse_topic_name(signal_name); if (!this->template remove_signal(parsed_signal_name, this->inputs_)) { - RCLCPP_DEBUG_STREAM(this->get_logger(), + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Unknown input '" << signal_name << "' (parsed name was '" << parsed_signal_name << "')."); } } } -template -inline void ComponentInterface::remove_output(const std::string& signal_name) { +inline void ComponentInterface::remove_output(const std::string& signal_name) { if (!this->template remove_signal(signal_name, this->outputs_)) { auto parsed_signal_name = utilities::parse_topic_name(signal_name); if (!this->template remove_signal(parsed_signal_name, this->outputs_)) { - RCLCPP_DEBUG_STREAM(this->get_logger(), + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Unknown output '" << signal_name << "' (parsed name was '" << parsed_signal_name << "')."); } } } -template -inline void ComponentInterface::declare_signal( +inline void ComponentInterface::declare_signal( const std::string& signal_name, const std::string& type, const std::string& default_topic, bool fixed_topic ) { std::string parsed_signal_name = utilities::parse_topic_name(signal_name); @@ -936,43 +887,39 @@ inline void ComponentInterface::declare_signal( } std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; auto parameter_name = parsed_signal_name + "_topic"; - if (NodeT::has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { + if (this->node_parameters_->has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { this->set_parameter_value(parameter_name, topic_name); } else { this->add_parameter( parameter_name, topic_name, "Signal topic name of " + type + " '" + parsed_signal_name + "'", fixed_topic); } - RCLCPP_DEBUG_STREAM(this->get_logger(), + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Declared signal '" << parsed_signal_name << "' and parameter '" << parameter_name << "' with value '" << topic_name << "'."); } -template -inline void ComponentInterface::declare_input( +inline void ComponentInterface::declare_input( const std::string& signal_name, const std::string& default_topic, bool fixed_topic ) { this->declare_signal(signal_name, "input", default_topic, fixed_topic); } -template -inline void ComponentInterface::declare_output( +inline void ComponentInterface::declare_output( const std::string& signal_name, const std::string& default_topic, bool fixed_topic ) { this->declare_signal(signal_name, "output", default_topic, fixed_topic); } -template template -inline void ComponentInterface::add_input( +inline void ComponentInterface::add_input( const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic, bool fixed_topic ) { this->add_input(signal_name, data, [] {}, default_topic, fixed_topic); } -template template -inline void ComponentInterface::add_input( +inline void ComponentInterface::add_input( const std::string& signal_name, const std::shared_ptr& data, const std::function& user_callback, const std::string& default_topic, bool fixed_topic ) { @@ -985,64 +932,70 @@ inline void ComponentInterface::add_input( } this->declare_input(parsed_signal_name, default_topic, fixed_topic); auto topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); - RCLCPP_DEBUG_STREAM(this->get_logger(), + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding input '" << parsed_signal_name << "' with topic name '" << topic_name << "'."); - auto message_pair = make_shared_message_pair(data, this->get_clock()); + auto message_pair = make_shared_message_pair(data, this->node_clock_->get_clock()); std::shared_ptr subscription_interface; switch (message_pair->get_type()) { case MessageType::BOOL: { auto subscription_handler = std::make_shared>(message_pair); - auto subscription = NodeT::template create_subscription( - topic_name, this->qos_, subscription_handler->get_callback(user_callback)); + auto subscription = rclcpp::create_subscription( + this->node_parameters_, this->node_topics_, topic_name, this->qos_, + subscription_handler->get_callback(user_callback)); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; } case MessageType::FLOAT64: { auto subscription_handler = std::make_shared>(message_pair); - auto subscription = NodeT::template create_subscription( - topic_name, this->qos_, subscription_handler->get_callback(user_callback)); + auto subscription = rclcpp::create_subscription( + this->node_parameters_, this->node_topics_, topic_name, this->qos_, + subscription_handler->get_callback(user_callback)); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; } case MessageType::FLOAT64_MULTI_ARRAY: { auto subscription_handler = std::make_shared>(message_pair); - auto subscription = NodeT::template create_subscription( - topic_name, this->qos_, subscription_handler->get_callback(user_callback)); + auto subscription = rclcpp::create_subscription( + this->node_parameters_, this->node_topics_, topic_name, this->qos_, + subscription_handler->get_callback(user_callback)); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; } case MessageType::INT32: { auto subscription_handler = std::make_shared>(message_pair); - auto subscription = NodeT::template create_subscription( - topic_name, this->qos_, subscription_handler->get_callback(user_callback)); + auto subscription = rclcpp::create_subscription( + this->node_parameters_, this->node_topics_, topic_name, this->qos_, + subscription_handler->get_callback(user_callback)); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; } case MessageType::STRING: { auto subscription_handler = std::make_shared>(message_pair); - auto subscription = NodeT::template create_subscription( - topic_name, this->qos_, subscription_handler->get_callback(user_callback)); + auto subscription = rclcpp::create_subscription( + this->node_parameters_, this->node_topics_, topic_name, this->qos_, + subscription_handler->get_callback(user_callback)); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; } case MessageType::ENCODED_STATE: { auto subscription_handler = std::make_shared>(message_pair); - auto subscription = NodeT::template create_subscription( - topic_name, this->qos_, subscription_handler->get_callback(user_callback)); + auto subscription = rclcpp::create_subscription( + this->node_parameters_, this->node_topics_, topic_name, this->qos_, + subscription_handler->get_callback(user_callback)); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; } } this->inputs_.insert_or_assign(parsed_signal_name, subscription_interface); } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add input '" << signal_name << "': " << ex.what()); + RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), + "Failed to add input '" << signal_name << "': " << ex.what()); } } -template template -inline void ComponentInterface::add_input( +inline void ComponentInterface::add_input( const std::string& signal_name, const std::function)>& callback, const std::string& default_topic, bool fixed_topic ) { @@ -1051,19 +1004,20 @@ inline void ComponentInterface::add_input( std::string parsed_signal_name = utilities::parse_topic_name(signal_name); this->declare_input(parsed_signal_name, default_topic, fixed_topic); auto topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); - RCLCPP_DEBUG_STREAM(this->get_logger(), + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding input '" << parsed_signal_name << "' with topic name '" << topic_name << "'."); - auto subscription = NodeT::template create_subscription(topic_name, this->qos_, callback); + auto subscription = rclcpp::create_subscription( + this->node_parameters_, this->node_topics_, topic_name, this->qos_, callback); auto subscription_interface = std::make_shared>()->create_subscription_interface(subscription); this->inputs_.insert_or_assign(parsed_signal_name, subscription_interface); } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add input '" << signal_name << "': " << ex.what()); + RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), + "Failed to add input '" << signal_name << "': " << ex.what()); } } -template -inline std::string ComponentInterface::validate_service_name(const std::string& service_name) { +inline std::string ComponentInterface::validate_service_name(const std::string& service_name) { std::string parsed_service_name = utilities::parse_topic_name(service_name); if (parsed_service_name.empty()) { throw exceptions::AddServiceException( @@ -1077,15 +1031,14 @@ inline std::string ComponentInterface::validate_service_name(const std::s return parsed_service_name; } -template -inline void ComponentInterface::add_service( +inline void ComponentInterface::add_service( const std::string& service_name, const std::function& callback ) { try { std::string parsed_service_name = this->validate_service_name(service_name); - RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding empty service '" << parsed_service_name << "'."); - auto service = NodeT::template create_service( - "~/" + parsed_service_name, [callback]( + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding empty service '" << parsed_service_name << "'."); + auto service = rclcpp::create_service( + this->node_base_, this->node_services_, "~/" + parsed_service_name, [callback]( const std::shared_ptr, std::shared_ptr response ) { @@ -1097,22 +1050,22 @@ inline void ComponentInterface::add_service( response->success = false; response->message = ex.what(); } - }); + }, this->qos_, this->cb_group_); this->empty_services_.insert_or_assign(parsed_service_name, service); } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add service '" << service_name << "': " << ex.what()); + RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), + "Failed to add service '" << service_name << "': " << ex.what()); } } -template -inline void ComponentInterface::add_service( +inline void ComponentInterface::add_service( const std::string& service_name, const std::function& callback ) { try { std::string parsed_service_name = this->validate_service_name(service_name); - RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding string service '" << parsed_service_name << "'."); - auto service = NodeT::template create_service( - "~/" + parsed_service_name, [callback]( + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding string service '" << parsed_service_name << "'."); + auto service = rclcpp::create_service( + this->node_base_, this->node_services_, "~/" + parsed_service_name, [callback]( const std::shared_ptr request, std::shared_ptr response ) { @@ -1124,95 +1077,89 @@ inline void ComponentInterface::add_service( response->success = false; response->message = ex.what(); } - }); + }, this->qos_, this->cb_group_); this->string_services_.insert_or_assign(parsed_service_name, service); } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add service '" << service_name << "': " << ex.what()); + RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), + "Failed to add service '" << service_name << "': " << ex.what()); } } -template -inline void -ComponentInterface::add_periodic_callback(const std::string& name, const std::function& callback) { +inline void ComponentInterface::add_periodic_callback(const std::string& name, const std::function& callback) { if (name.empty()) { - RCLCPP_ERROR(this->get_logger(), "Failed to add periodic function: Provide a non empty string as a name."); + RCLCPP_ERROR(this->node_logging_->get_logger(), + "Failed to add periodic function: Provide a non empty string as a name."); return; } if (this->periodic_callbacks_.find(name) != this->periodic_callbacks_.end()) { - RCLCPP_WARN_STREAM(this->get_logger(), "Periodic function '" << name << "' already exists, overwriting."); + RCLCPP_WARN_STREAM(this->node_logging_->get_logger(), + "Periodic function '" << name << "' already exists, overwriting."); } else { - RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding periodic function '" << name << "'."); + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding periodic function '" << name << "'."); } this->periodic_callbacks_.template insert_or_assign(name, callback); } -template -inline void ComponentInterface::add_tf_broadcaster() { +inline void ComponentInterface::add_tf_broadcaster() { if (this->tf_broadcaster_ == nullptr) { - RCLCPP_DEBUG(this->get_logger(), "Adding TF broadcaster."); + RCLCPP_DEBUG(this->node_logging_->get_logger(), "Adding TF broadcaster."); console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); - this->tf_broadcaster_ = std::make_shared(*this); + this->tf_broadcaster_ = std::make_shared( + this->node_parameters_, this->node_topics_, tf2_ros::DynamicBroadcasterQoS()); } else { - RCLCPP_DEBUG(this->get_logger(), "TF broadcaster already exists."); + RCLCPP_DEBUG(this->node_logging_->get_logger(), "TF broadcaster already exists."); } } -template -inline void ComponentInterface::add_static_tf_broadcaster() { +inline void ComponentInterface::add_static_tf_broadcaster() { if (this->static_tf_broadcaster_ == nullptr) { - RCLCPP_DEBUG(this->get_logger(), "Adding static TF broadcaster."); + RCLCPP_DEBUG(this->node_logging_->get_logger(), "Adding static TF broadcaster."); console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); tf2_ros::StaticBroadcasterQoS qos; rclcpp::PublisherOptionsWithAllocator> options; - this->static_tf_broadcaster_ = std::make_shared(*this, qos, options); + this->static_tf_broadcaster_ = + std::make_shared(this->node_parameters_, this->node_topics_, qos, options); } else { - RCLCPP_DEBUG(this->get_logger(), "Static TF broadcaster already exists."); + RCLCPP_DEBUG(this->node_logging_->get_logger(), "Static TF broadcaster already exists."); } } -template -inline void ComponentInterface::add_tf_listener() { +inline void ComponentInterface::add_tf_listener() { if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { - RCLCPP_DEBUG(this->get_logger(), "Adding TF buffer and listener."); + RCLCPP_DEBUG(this->node_logging_->get_logger(), "Adding TF buffer and listener."); console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); - this->tf_buffer_ = std::make_shared(this->get_clock()); + this->tf_buffer_ = std::make_shared(this->node_clock_->get_clock()); this->tf_listener_ = std::make_shared(*this->tf_buffer_); } else { - RCLCPP_DEBUG(this->get_logger(), "TF buffer and listener already exist."); + RCLCPP_DEBUG(this->node_logging_->get_logger(), "TF buffer and listener already exist."); } } -template -inline void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { +inline void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { this->send_transforms(std::vector{transform}); } -template -inline void -ComponentInterface::send_transforms(const std::vector& transforms) { +inline void ComponentInterface::send_transforms(const std::vector& transforms) { this->template publish_transforms(transforms, this->tf_broadcaster_); } -template -inline void ComponentInterface::send_static_transform(const state_representation::CartesianPose& transform) { +inline void ComponentInterface::send_static_transform(const state_representation::CartesianPose& transform) { this->send_static_transforms(std::vector{transform}); } -template inline void -ComponentInterface::send_static_transforms(const std::vector& transforms) { +ComponentInterface::send_static_transforms(const std::vector& transforms) { this->template publish_transforms(transforms, this->static_tf_broadcaster_, true); } -template template -inline void ComponentInterface::publish_transforms( +inline void ComponentInterface::publish_transforms( const std::vector& transforms, const std::shared_ptr& tf_broadcaster, bool is_static ) { std::string modifier = is_static ? "static " : ""; if (tf_broadcaster == nullptr) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, "Failed to send " << modifier << "transform: No " << modifier << "TF broadcaster configured."); return; @@ -1222,18 +1169,18 @@ inline void ComponentInterface::publish_transforms( transform_messages.reserve(transforms.size()); for (const auto& tf : transforms) { geometry_msgs::msg::TransformStamped transform_message; - modulo_core::translators::write_message(transform_message, tf, this->get_clock()->now()); + modulo_core::translators::write_message( + transform_message, tf, this->node_clock_->get_clock()->now()); transform_messages.emplace_back(transform_message); } tf_broadcaster->sendTransform(transform_messages); } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, "Failed to send " << modifier << "transform: " << ex.what()); } } -template -inline geometry_msgs::msg::TransformStamped ComponentInterface::lookup_ros_transform( +inline geometry_msgs::msg::TransformStamped ComponentInterface::lookup_ros_transform( const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point, const tf2::Duration& duration ) { @@ -1247,8 +1194,7 @@ inline geometry_msgs::msg::TransformStamped ComponentInterface::lookup_ro } } -template -inline state_representation::CartesianPose ComponentInterface::lookup_transform( +inline state_representation::CartesianPose ComponentInterface::lookup_transform( const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point, const tf2::Duration& duration ) { @@ -1258,13 +1204,13 @@ inline state_representation::CartesianPose ComponentInterface::lookup_tra return result; } -template -inline state_representation::CartesianPose ComponentInterface::lookup_transform( +inline state_representation::CartesianPose ComponentInterface::lookup_transform( const std::string& frame, const std::string& reference_frame, double validity_period, const tf2::Duration& duration ) { auto transform = this->lookup_ros_transform(frame, reference_frame, tf2::TimePoint(std::chrono::microseconds(0)), duration); - if (validity_period > 0.0 && (this->get_clock()->now() - transform.header.stamp).seconds() > validity_period) { + if (validity_period > 0.0 + && (this->node_clock_->get_clock()->now() - transform.header.stamp).seconds() > validity_period) { throw exceptions::LookupTransformException("Failed to lookup transform: Latest transform is too old!"); } state_representation::CartesianPose result(frame, reference_frame); @@ -1272,24 +1218,21 @@ inline state_representation::CartesianPose ComponentInterface::lookup_tra return result; } -template -inline void ComponentInterface::publish_predicate(const std::string& name) { +inline void ComponentInterface::publish_predicate(const std::string& name) { modulo_component_interfaces::msg::Predicate message; - message.component = this->get_node_base_interface()->get_fully_qualified_name(); + message.component = this->node_base_->get_fully_qualified_name(); message.predicate = name; message.value = this->get_predicate(name); this->predicate_publisher_->publish(message); } -template -inline void ComponentInterface::publish_predicates() { +inline void ComponentInterface::publish_predicates() { for (const auto& predicate : this->predicates_) { this->publish_predicate(predicate.first); } } -template -inline void ComponentInterface::publish_output(const std::string& signal_name) { +inline void ComponentInterface::publish_output(const std::string& signal_name) { auto parsed_signal_name = utilities::parse_topic_name(signal_name); if (this->outputs_.find(parsed_signal_name) == this->outputs_.cend()) { throw exceptions::ComponentException("Output with name '" + signal_name + "' doesn't exist."); @@ -1300,42 +1243,39 @@ inline void ComponentInterface::publish_output(const std::string& signal_ try { this->outputs_.at(parsed_signal_name)->publish(); } catch (const modulo_core::exceptions::CoreException& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, "Failed to publish output '" << parsed_signal_name << "': " << ex.what()); } } -template -inline void ComponentInterface::publish_outputs() { +inline void ComponentInterface::publish_outputs() { for (const auto& [signal, publisher] : this->outputs_) { try { if (this->periodic_outputs_.at(signal)) { publisher->publish(); } } catch (const modulo_core::exceptions::CoreException& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, "Failed to publish output '" << signal << "': " << ex.what()); } } } -template -inline void ComponentInterface::evaluate_periodic_callbacks() { +inline void ComponentInterface::evaluate_periodic_callbacks() { for (const auto& [name, callback] : this->periodic_callbacks_) { try { callback(); } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, "Failed to evaluate periodic function callback '" << name << "': " << ex.what()); } } } -template template -inline std::string ComponentInterface::create_output( - const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic, - bool fixed_topic, bool publish_on_step +inline std::string ComponentInterface::create_output( + modulo_core::communication::PublisherType publisher_type, const std::string& signal_name, + const std::shared_ptr& data, const std::string& default_topic, bool fixed_topic, bool publish_on_step ) { using namespace modulo_core::communication; try { @@ -1345,12 +1285,12 @@ inline std::string ComponentInterface::create_output( "Invalid data pointer for output '" + parsed_signal_name + "'."); } this->declare_output(parsed_signal_name, default_topic, fixed_topic); - RCLCPP_DEBUG_STREAM(this->get_logger(), + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Creating output '" << parsed_signal_name << "' (provided signal name was '" << signal_name << "')."); - auto message_pair = make_shared_message_pair(data, this->get_clock()); + auto message_pair = make_shared_message_pair(data, this->node_clock_->get_clock()); this->outputs_.insert_or_assign( - parsed_signal_name, std::make_shared(this->publisher_type_, message_pair)); + parsed_signal_name, std::make_shared(publisher_type, message_pair)); this->periodic_outputs_.insert_or_assign(parsed_signal_name, publish_on_step); return parsed_signal_name; } catch (const exceptions::AddSignalException&) { @@ -1360,19 +1300,16 @@ inline std::string ComponentInterface::create_output( } } -template -inline void ComponentInterface::raise_error() { - RCLCPP_DEBUG(this->get_logger(), "raise_error called: Setting predicate 'in_error_state' to true."); +inline void ComponentInterface::raise_error() { + RCLCPP_DEBUG(this->node_logging_->get_logger(), "raise_error called: Setting predicate 'in_error_state' to true."); this->set_predicate("in_error_state", true); } -template -inline rclcpp::QoS ComponentInterface::get_qos() const { +inline rclcpp::QoS ComponentInterface::get_qos() const { return this->qos_; } -template -inline void ComponentInterface::set_qos(const rclcpp::QoS& qos) { +inline void ComponentInterface::set_qos(const rclcpp::QoS& qos) { this->qos_ = qos; } }// namespace modulo_components diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp new file mode 100644 index 000000000..8c7cfd310 --- /dev/null +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -0,0 +1,35 @@ +#include "modulo_components/ComponentInterface.hpp" + +using namespace modulo_core::communication; + +namespace modulo_components { + +ComponentInterface::ComponentInterface( + const std::shared_ptr>& interfaces +) : + node_base_(interfaces->get_node_base_interface()), + node_clock_(interfaces->get_node_clock_interface()), + node_logging_(interfaces->get_node_logging_interface()), + node_parameters_(interfaces->get_node_parameters_interface()), + node_services_(interfaces->get_node_services_interface()), + node_timers_(interfaces->get_node_timers_interface()), + node_topics_(interfaces->get_node_topics_interface()) { + this->cb_group_ = node_base_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + // register the parameter change callback handler + this->parameter_cb_handle_ = this->node_parameters_->add_on_set_parameters_callback( + [this](const std::vector& 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->predicate_publisher_ = rclcpp::create_publisher( + this->node_parameters_, this->node_topics_, "/predicates", this->qos_); + + this->add_predicate("in_error_state", false); + + this->step_timer_ = rclcpp::create_wall_timer( + std::chrono::nanoseconds(static_cast(this->get_parameter_value("period") * 1e9)), + [this] { this->step(); }, this->cb_group_, this->node_base_.get(), this->node_timers_.get()); +} +}// namespace modulo_components diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 06f41e9a8..8f9d912a6 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -8,49 +8,55 @@ using namespace state_representation; namespace modulo_components { -template -class ComponentInterfacePublicInterface : public ComponentInterface { +class ComponentInterfacePublicInterface : public ComponentInterface { public: - explicit ComponentInterfacePublicInterface( - const rclcpp::NodeOptions& node_options, modulo_core::communication::PublisherType publisher_type, - const std::string& fallback_name = "ComponentInterfacePublicInterface" - ) : ComponentInterface(node_options, publisher_type, fallback_name) {} - using ComponentInterface::add_parameter; - using ComponentInterface::get_parameter; - using ComponentInterface::get_parameter_value; - using ComponentInterface::set_parameter_value; - using ComponentInterface::parameter_map_; - using ComponentInterface::add_predicate; - using ComponentInterface::get_predicate; - using ComponentInterface::set_predicate; - using ComponentInterface::predicates_; - using ComponentInterface::add_trigger; - using ComponentInterface::trigger; - using ComponentInterface::triggers_; - using ComponentInterface::declare_input; - using ComponentInterface::declare_output; - using ComponentInterface::remove_input; - using ComponentInterface::add_input; - using ComponentInterface::add_service; - using ComponentInterface::inputs_; - using ComponentInterface::create_output; - using ComponentInterface::outputs_; - using ComponentInterface::periodic_outputs_; - using ComponentInterface::empty_services_; - using ComponentInterface::string_services_; - using ComponentInterface::add_tf_broadcaster; - using ComponentInterface::add_static_tf_broadcaster; - using ComponentInterface::add_tf_listener; - using ComponentInterface::publish_output; - using ComponentInterface::send_transform; - using ComponentInterface::send_transforms; - using ComponentInterface::send_static_transform; - using ComponentInterface::send_static_transforms; - using ComponentInterface::lookup_transform; - using ComponentInterface::raise_error; - using ComponentInterface::get_qos; - using ComponentInterface::set_qos; - using ComponentInterface::get_clock; + template + explicit ComponentInterfacePublicInterface(const std::shared_ptr& node) : ComponentInterface( + std::make_shared>( + 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(), + node->get_node_services_interface(), node->get_node_time_source_interface(), + node->get_node_timers_interface(), node->get_node_topics_interface(), + node->get_node_waitables_interface())) {} + using ComponentInterface::node_base_; + using ComponentInterface::node_clock_; + using ComponentInterface::node_logging_; + using ComponentInterface::node_parameters_; + using ComponentInterface::add_parameter; + using ComponentInterface::get_parameter; + using ComponentInterface::get_parameter_value; + using ComponentInterface::set_parameter_value; + using ComponentInterface::parameter_map_; + using ComponentInterface::add_predicate; + using ComponentInterface::get_predicate; + using ComponentInterface::set_predicate; + using ComponentInterface::predicates_; + using ComponentInterface::add_trigger; + using ComponentInterface::trigger; + using ComponentInterface::triggers_; + using ComponentInterface::declare_input; + using ComponentInterface::declare_output; + using ComponentInterface::remove_input; + using ComponentInterface::add_input; + using ComponentInterface::add_service; + using ComponentInterface::inputs_; + using ComponentInterface::create_output; + using ComponentInterface::outputs_; + using ComponentInterface::periodic_outputs_; + using ComponentInterface::empty_services_; + using ComponentInterface::string_services_; + using ComponentInterface::add_tf_broadcaster; + using ComponentInterface::add_static_tf_broadcaster; + using ComponentInterface::add_tf_listener; + using ComponentInterface::publish_output; + using ComponentInterface::send_transform; + using ComponentInterface::send_transforms; + using ComponentInterface::send_static_transform; + using ComponentInterface::send_static_transforms; + using ComponentInterface::lookup_transform; + using ComponentInterface::raise_error; + using ComponentInterface::get_qos; + using ComponentInterface::set_qos; bool on_validate_parameter_callback(const std::shared_ptr&) override { validate_parameter_was_called = true; @@ -58,15 +64,16 @@ class ComponentInterfacePublicInterface : public ComponentInterface { } rclcpp::Parameter get_ros_parameter(const std::string& name) { - return NodeT::get_parameter(name); + return this->node_parameters_->get_parameter(name); } rcl_interfaces::msg::SetParametersResult set_ros_parameter(const rclcpp::Parameter& parameter) { - return NodeT::set_parameter(parameter); + return this->node_parameters_->set_parameters({parameter}).at( + 0); } std::string get_parameter_description(const std::string& name) { - return NodeT::describe_parameter(name).description; + return this->node_parameters_->describe_parameters({name}).at(0).description; } bool validate_parameter_was_called = false; diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 0fb9c7002..fc86c3fde 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -11,17 +11,6 @@ namespace modulo_components { using namespace std::chrono_literals; -template -std::shared_ptr> make_component_interface(const rclcpp::NodeOptions& options) { - if (std::is_same::value) { - return std::make_shared>( - options, modulo_core::communication::PublisherType::PUBLISHER); - } else if (std::is_same::value) { - return std::make_shared>( - options, modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER); - } -} - template class ComponentInterfaceTest : public ::testing::Test { protected: @@ -34,40 +23,49 @@ class ComponentInterfaceTest : public ::testing::Test { } void SetUp() override { - this->exec_ = std::make_shared(); - this->component_ = make_component_interface(rclcpp::NodeOptions()); + exec_ = std::make_shared(); + this->node_ = std::make_shared("ComponentInterfacePublicInterface", rclcpp::NodeOptions()); + this->component_ = std::make_shared(this->node_); + if (std::is_same::value) { + this->pub_type_ = modulo_core::communication::PublisherType::PUBLISHER; + } else if (std::is_same::value) { + this->pub_type_ = modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER; + } } std::shared_ptr exec_; - std::shared_ptr> component_; + std::shared_ptr component_; + std::shared_ptr node_; + modulo_core::communication::PublisherType pub_type_; }; using NodeTypes = ::testing::Types; TYPED_TEST_SUITE(ComponentInterfaceTest, NodeTypes); -TYPED_TEST(ComponentInterfaceTest, RatePeriodParameters) { - std::shared_ptr> component; - auto node_options = rclcpp::NodeOptions(); - component = make_component_interface(node_options); - EXPECT_EQ(component->template get_parameter_value("rate"), 10); - EXPECT_EQ(component->template get_parameter_value("period"), 0.1); - - node_options = rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("rate", 200)}); - component = make_component_interface(node_options); - EXPECT_EQ(component->template get_parameter_value("rate"), 200); - EXPECT_EQ(component->template get_parameter_value("period"), 0.005); - - node_options = rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("period", 0.01)}); - component = make_component_interface(node_options); - EXPECT_EQ(component->template get_parameter_value("rate"), 100); - EXPECT_EQ(component->template get_parameter_value("period"), 0.01); - - node_options = - rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("rate", 200), rclcpp::Parameter("period", 0.01)}); - component = make_component_interface(node_options); - EXPECT_EQ(component->template get_parameter_value("rate"), 200); - EXPECT_EQ(component->template get_parameter_value("period"), 0.005); -} +// TODO this needs to be tested on component level now +// TYPED_TEST(ComponentInterfaceTest, RatePeriodParameters) { +// std::shared_ptr component; +// auto node_options = rclcpp::NodeOptions(); +// component = make_component_interface(node_options); +// EXPECT_EQ(component->template get_parameter_value("rate"), 10); +// EXPECT_EQ(component->template get_parameter_value("period"), 0.1); + +// node_options = rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("rate", 200)}); +// component = make_component_interface(node_options); +// EXPECT_EQ(component->template get_parameter_value("rate"), 200); +// EXPECT_EQ(component->template get_parameter_value("period"), 0.005); + +// node_options = rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("period", 0.01)}); +// component = make_component_interface(node_options); +// EXPECT_EQ(component->template get_parameter_value("rate"), 100); +// EXPECT_EQ(component->template get_parameter_value("period"), 0.01); + +// node_options = +// rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("rate", 200), rclcpp::Parameter("period", 0.01)}); +// component = make_component_interface(node_options); +// EXPECT_EQ(component->template get_parameter_value("rate"), 200); +// EXPECT_EQ(component->template get_parameter_value("period"), 0.005); +// } TYPED_TEST(ComponentInterfaceTest, AddBoolPredicate) { this->component_->add_predicate("foo", true); @@ -154,7 +152,8 @@ TYPED_TEST(ComponentInterfaceTest, AddInputWithUserCallback) { this->component_->inputs_.at("state")->template get_handler()->get_callback(); state_representation::CartesianState new_state("B"); auto message = std::make_shared(); - modulo_core::translators::write_message(*message, new_state, this->component_->get_clock()->now()); + modulo_core::translators::write_message( + *message, new_state, this->component_->node_clock_->get_clock()->now()); EXPECT_STREQ(state_data->get_name().c_str(), "A"); EXPECT_NO_THROW(callback(message)); EXPECT_TRUE(callback_triggered); @@ -216,8 +215,8 @@ TYPED_TEST(ComponentInterfaceTest, CallEmptyService) { EXPECT_NO_THROW(this->component_->add_service("empty", empty_callback)); auto client = std::make_shared>( - rclcpp::NodeOptions(), "/" + std::string(this->component_->get_name()) + "/empty"); - this->exec_->add_node(this->component_->get_node_base_interface()); + rclcpp::NodeOptions(), "/" + std::string(this->component_->node_base_->get_name()) + "/empty"); + this->exec_->add_node(this->component_->node_base_); this->exec_->add_node(client); auto request = std::make_shared(); auto future = client->call_async(request); @@ -236,8 +235,8 @@ TYPED_TEST(ComponentInterfaceTest, CallStringService) { EXPECT_NO_THROW(this->component_->add_service("string", string_callback)); auto client = std::make_shared>( - rclcpp::NodeOptions(), "/" + std::string(this->component_->get_name()) + "/string"); - this->exec_->add_node(this->component_->get_node_base_interface()); + rclcpp::NodeOptions(), "/" + std::string(this->component_->node_base_->get_name()) + "/string"); + this->exec_->add_node(this->component_->node_base_); this->exec_->add_node(client); auto request = std::make_shared(); request->payload = "payload"; @@ -251,7 +250,7 @@ TYPED_TEST(ComponentInterfaceTest, CallStringService) { TYPED_TEST(ComponentInterfaceTest, CreateOutput) { auto data = std::make_shared(true); - EXPECT_NO_THROW(this->component_->create_output("test", data, "/topic", true, true)); + EXPECT_NO_THROW(this->component_->create_output(this->pub_type_, "test", data, "/topic", true, true)); EXPECT_FALSE(this->component_->outputs_.find("test") == this->component_->outputs_.end()); EXPECT_EQ(this->component_->template get_parameter_value("test_topic"), "/topic"); EXPECT_TRUE(this->component_->periodic_outputs_.at("test")); @@ -265,7 +264,7 @@ TYPED_TEST(ComponentInterfaceTest, CreateOutput) { EXPECT_EQ(pub_interface->get_message_pair()->get_type(), modulo_core::communication::MessageType::BOOL); EXPECT_THROW(pub_interface->publish(), modulo_core::exceptions::CoreException); - EXPECT_NO_THROW(this->component_->create_output("_tEsT_#1@3", data, "", true, false)); + EXPECT_NO_THROW(this->component_->create_output(this->pub_type_, "_tEsT_#1@3", data, "", true, false)); EXPECT_FALSE(this->component_->periodic_outputs_.at("test_13")); EXPECT_NO_THROW(this->component_->publish_output("_tEsT_#1@3")); EXPECT_NO_THROW(this->component_->publish_output("test_13")); diff --git a/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp index d4a52cbae..e31f9ce12 100644 --- a/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp @@ -1,19 +1,17 @@ #include #include "modulo_components/exceptions/ComponentParameterException.hpp" -#include "modulo_core/EncodedState.hpp" #include "test_modulo_components/component_public_interfaces.hpp" namespace modulo_components { template -class EmptyParameterInterface : public ComponentInterfacePublicInterface { +class EmptyParameterInterface : public ComponentInterfacePublicInterface { public: explicit EmptyParameterInterface( - const rclcpp::NodeOptions& node_options, modulo_core::communication::PublisherType publisher_type, - const std::string& fallback_name = "EmptyParameterInterface", bool allow_empty = true, bool add_parameter = true, + const std::shared_ptr& node, bool allow_empty = true, bool add_parameter = true, bool empty_parameter = true - ) : ComponentInterfacePublicInterface(node_options, publisher_type, fallback_name), allow_empty_(allow_empty) { + ) : ComponentInterfacePublicInterface(node), allow_empty_(allow_empty) { if (add_parameter) { if (empty_parameter) { this->add_parameter(std::make_shared>("name"), "Test parameter"); @@ -30,7 +28,7 @@ class EmptyParameterInterface : public ComponentInterfacePublicInterface if (parameter->is_empty()) { return this->allow_empty_; } else if (parameter->get_parameter_value().empty()) { - RCLCPP_ERROR(this->get_logger(), "Provide a non empty value for parameter 'name'"); + RCLCPP_ERROR(this->node_logging_->get_logger(), "Provide a non empty value for parameter 'name'"); return false; } } @@ -52,13 +50,8 @@ class ComponentInterfaceEmptyParameterTest : public ::testing::Test { } void SetUp() override { - if (std::is_same::value) { - this->component_ = std::make_shared>( - rclcpp::NodeOptions(), modulo_core::communication::PublisherType::PUBLISHER); - } else if (std::is_same::value) { - this->component_ = std::make_shared>( - rclcpp::NodeOptions(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER); - } + this->component_ = std::make_shared>( + std::make_shared("EmptyParameterInterface", rclcpp::NodeOptions())); } std::shared_ptr> component_; @@ -67,28 +60,15 @@ using NodeTypes = ::testing::Types::value) { - EXPECT_THROW(std::make_shared>( - rclcpp::NodeOptions(), modulo_core::communication::PublisherType::PUBLISHER, "EmptyParameterComponent", false), - modulo_components::exceptions::ComponentParameterException); - } else if (std::is_same::value) { - EXPECT_THROW(std::make_shared>( - rclcpp::NodeOptions(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, - "EmptyParameterComponent", false), modulo_components::exceptions::ComponentParameterException); - } + auto node = std::make_shared("EmptyParameterComponent", rclcpp::NodeOptions()); + EXPECT_THROW(std::make_shared>(node, false), + modulo_components::exceptions::ComponentParameterException); } TYPED_TEST(ComponentInterfaceEmptyParameterTest, NotAllowEmpty) { std::shared_ptr> component; - if (std::is_same::value) { - component = std::make_shared>( - rclcpp::NodeOptions(), modulo_core::communication::PublisherType::PUBLISHER, "EmptyParameterComponent", false, - false); - } else if (std::is_same::value) { - component = std::make_shared>( - rclcpp::NodeOptions(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, - "EmptyParameterComponent", false, false); - } + auto node = std::make_shared("EmptyParameterComponent", rclcpp::NodeOptions()); + component = std::make_shared>(node, false, false); EXPECT_THROW(component->add_parameter(std::make_shared>("name"), "Test parameter"), exceptions::ComponentParameterException); EXPECT_THROW(auto param = component->get_parameter("name"), exceptions::ComponentParameterException); @@ -96,16 +76,8 @@ TYPED_TEST(ComponentInterfaceEmptyParameterTest, NotAllowEmpty) { } TYPED_TEST(ComponentInterfaceEmptyParameterTest, AllowEmpty) { - std::shared_ptr> component; - if (std::is_same::value) { - component = std::make_shared>( - rclcpp::NodeOptions(), modulo_core::communication::PublisherType::PUBLISHER, "EmptyParameterComponent", true, - false); - } else if (std::is_same::value) { - component = std::make_shared>( - rclcpp::NodeOptions(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, - "EmptyParameterComponent", true, false); - } + auto node = std::make_shared("EmptyParameterInterface", rclcpp::NodeOptions()); + auto component = std::make_shared>(node, true, false); EXPECT_NO_THROW(component->add_parameter(std::make_shared>("name"), "Test parameter")); EXPECT_EQ(component->get_parameter("name")->get_parameter_type(), ParameterType::STRING); EXPECT_TRUE(component->get_parameter("name")->is_empty()); @@ -168,7 +140,7 @@ TYPED_TEST(ComponentInterfaceEmptyParameterTest, ValidateEmptyParameter) { TYPED_TEST(ComponentInterfaceEmptyParameterTest, ChangeParameterType) { // Add parameter from component interface this->component_->add_parameter(std::make_shared>("int"), "Test parameter"); - EXPECT_TRUE(this->component_->describe_parameter("int").dynamic_typing); + EXPECT_TRUE(this->component_->node_parameters_->describe_parameters({"int"}).at(0).dynamic_typing); this->component_->template set_parameter_value("int", 1); EXPECT_EQ(this->component_->get_parameter("int")->get_parameter_type(), ParameterType::INT); EXPECT_EQ(this->component_->get_parameter("int")->template get_parameter_value(), 1); @@ -197,30 +169,16 @@ TYPED_TEST(ComponentInterfaceEmptyParameterTest, ChangeParameterType) { TYPED_TEST(ComponentInterfaceEmptyParameterTest, ParameterOverrides) { // Construction with not allowing empty parameters but providing the parameter override should succeed - if (std::is_same::value) { - EXPECT_NO_THROW(std::make_shared>( - rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("name", "test")}), - modulo_core::communication::PublisherType::PUBLISHER, "EmptyParameterComponent", false)); - } else if (std::is_same::value) { - EXPECT_NO_THROW(std::make_shared>( - rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("name", "test")}), - modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, "EmptyParameterComponent", false)); - } + auto node = std::make_shared( + "EmptyParameterInterface", rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("name", "test")})); + EXPECT_NO_THROW(std::make_shared>(node, false)); } TYPED_TEST(ComponentInterfaceEmptyParameterTest, ParameterOverridesEmpty) { // Construction with not allowing empty parameters and providing an uninitialized parameter override should not succeed - std::shared_ptr> component; - if (std::is_same::value) { - EXPECT_THROW(component = std::make_shared>( - rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("name")}), - modulo_core::communication::PublisherType::PUBLISHER, "EmptyParameterComponent", false, true, false), - modulo_components::exceptions::ComponentParameterException); - } else if (std::is_same::value) { - EXPECT_THROW(std::make_shared>( - rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("name")}), - modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, "EmptyParameterComponent", false, true, false), - modulo_components::exceptions::ComponentParameterException); - } + auto node = std::make_shared( + "EmptyParameterInterface", rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("name")})); + EXPECT_THROW(std::make_shared>(node, false, true, false), + modulo_components::exceptions::ComponentParameterException); } } // namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp index 8bac27aa3..011996157 100644 --- a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp @@ -18,13 +18,8 @@ class ComponentInterfaceParameterTest : public ::testing::Test { } void SetUp() override { - if (std::is_same::value) { - this->component_ = std::make_shared>( - rclcpp::NodeOptions(), modulo_core::communication::PublisherType::PUBLISHER); - } else if (std::is_same::value) { - this->component_ = std::make_shared>( - rclcpp::NodeOptions(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER); - } + this->component_ = std::make_shared( + std::make_shared("ComponentInterfacePublicInterface", rclcpp::NodeOptions())); param_ = state_representation::make_shared_parameter("test", 1); } @@ -35,7 +30,7 @@ class ComponentInterfaceParameterTest : public ::testing::Test { EXPECT_EQ(this->component_->parameter_map_.template get_parameter_value("test"), value); } - std::shared_ptr> component_; + std::shared_ptr component_; std::shared_ptr> param_; }; using NodeTypes = ::testing::Types; From 2d9d5eecff4dd64173278b78e30bd888aecbcfa9 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Tue, 24 Oct 2023 17:31:35 +0200 Subject: [PATCH 2/5] refactor: update component classes to inherit from new component interface (#38) Make qos private. --- CHANGELOG.md | 1 + .../include/modulo_components/Component.hpp | 34 ++++--- .../modulo_components/ComponentInterface.hpp | 3 +- .../modulo_components/LifecycleComponent.hpp | 26 ++--- source/modulo_components/src/Component.cpp | 12 ++- .../src/LifecycleComponent.cpp | 94 +++++++++---------- .../component_public_interfaces.hpp | 2 + .../test/cpp/test_component.cpp | 24 +++++ .../test/cpp/test_lifecycle_component.cpp | 24 +++++ 9 files changed, 138 insertions(+), 82 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index ca017e473..233d10c2d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ Release Versions: ## Upcoming changes (in development) +- Update component classes to inherit from new ComponentInterface (#38) - Refactor ComponentInterface to use node interfaces (#33) ## 3.2.0 diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 7c0c87377..0836ff07b 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -5,9 +5,6 @@ #include #include "modulo_components/ComponentInterface.hpp" -#include "modulo_components/utilities/utilities.hpp" -#include "modulo_core/EncodedState.hpp" -#include "modulo_core/exceptions/CoreException.hpp" namespace modulo_components { @@ -23,7 +20,7 @@ namespace modulo_components { * constructor. * @see LifecycleComponent for a state-based composition alternative */ -class Component : public ComponentInterface { +class Component : public rclcpp::Node, public ComponentInterface { public: friend class ComponentPublicInterface; @@ -79,13 +76,13 @@ class Component : public ComponentInterface { void on_execute(); // TODO hide ROS methods - using ComponentInterface::create_output; - using ComponentInterface::inputs_; - using ComponentInterface::outputs_; - using ComponentInterface::qos_; - using ComponentInterface::publish_predicates; - using ComponentInterface::publish_outputs; - using ComponentInterface::evaluate_periodic_callbacks; + using ComponentInterface::create_output; + using ComponentInterface::inputs_; + using ComponentInterface::outputs_; + using ComponentInterface::periodic_outputs_; + using ComponentInterface::publish_predicates; + using ComponentInterface::publish_outputs; + using ComponentInterface::evaluate_periodic_callbacks; std::thread execute_thread_; ///< The execution thread of the component bool started_; ///< Flag that indicates if execution has started or not @@ -98,28 +95,29 @@ inline void Component::add_output( ) { using namespace modulo_core::communication; try { - auto parsed_signal_name = this->create_output(signal_name, data, default_topic, fixed_topic, publish_on_step); + auto parsed_signal_name = + this->create_output(PublisherType::PUBLISHER, signal_name, data, default_topic, fixed_topic, publish_on_step); auto topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding output '" << parsed_signal_name << "' with topic name '" << topic_name << "'."); auto message_pair = this->outputs_.at(parsed_signal_name)->get_message_pair(); switch (message_pair->get_type()) { case MessageType::BOOL: { - auto publisher = this->create_publisher(topic_name, this->qos_); + auto publisher = this->create_publisher(topic_name, this->get_qos()); this->outputs_.at(parsed_signal_name) = std::make_shared, std_msgs::msg::Bool>>( PublisherType::PUBLISHER, publisher)->create_publisher_interface(message_pair); break; } case MessageType::FLOAT64: { - auto publisher = this->create_publisher(topic_name, this->qos_); + auto publisher = this->create_publisher(topic_name, this->get_qos()); this->outputs_.at(parsed_signal_name) = std::make_shared, std_msgs::msg::Float64>>( PublisherType::PUBLISHER, publisher)->create_publisher_interface(message_pair); break; } case MessageType::FLOAT64_MULTI_ARRAY: { - auto publisher = this->create_publisher(topic_name, this->qos_); + auto publisher = this->create_publisher(topic_name, this->get_qos()); this->outputs_.at(parsed_signal_name) = std::make_shared< PublisherHandler< rclcpp::Publisher, std_msgs::msg::Float64MultiArray>>( @@ -127,21 +125,21 @@ inline void Component::add_output( break; } case MessageType::INT32: { - auto publisher = this->create_publisher(topic_name, this->qos_); + auto publisher = this->create_publisher(topic_name, this->get_qos()); this->outputs_.at(parsed_signal_name) = std::make_shared, std_msgs::msg::Int32>>( PublisherType::PUBLISHER, publisher)->create_publisher_interface(message_pair); break; } case MessageType::STRING: { - auto publisher = this->create_publisher(topic_name, this->qos_); + auto publisher = this->create_publisher(topic_name, this->get_qos()); this->outputs_.at(parsed_signal_name) = std::make_shared, std_msgs::msg::String>>( PublisherType::PUBLISHER, publisher)->create_publisher_interface(message_pair); break; } case MessageType::ENCODED_STATE: { - auto publisher = this->create_publisher(topic_name, this->qos_); + auto publisher = this->create_publisher(topic_name, this->get_qos()); this->outputs_.at(parsed_signal_name) = std::make_shared, modulo_core::EncodedState>>( PublisherType::PUBLISHER, publisher)->create_publisher_interface(message_pair); diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index d13254854..10534c38a 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -462,8 +462,6 @@ class ComponentInterface { std::map> outputs_; ///< Map of outputs std::map periodic_outputs_; ///< Map of outputs with periodic publishing flag - rclcpp::QoS qos_ = rclcpp::QoS(10); ///< Quality of Service for ROS publishers and subscribers - private: /** * @brief Callback function to validate and update parameters on change. @@ -575,6 +573,7 @@ class ComponentInterface { std::shared_ptr node_services_; std::shared_ptr node_timers_; std::shared_ptr node_topics_; + rclcpp::QoS qos_ = rclcpp::QoS(10); ///< Quality of Service for ROS publishers and subscribers bool set_parameter_callback_called_ = false; ///< Flag to indicate if on_set_parameter_callback was called }; diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 3e8be7c72..441f5ecd0 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -4,7 +4,6 @@ #include #include "modulo_components/ComponentInterface.hpp" -#include "modulo_core/EncodedState.hpp" namespace modulo_components { @@ -27,7 +26,7 @@ namespace modulo_components { * - on_step_callback() * @see Component for a stateless composition alternative */ -class LifecycleComponent : public ComponentInterface { +class LifecycleComponent : public rclcpp_lifecycle::LifecycleNode, public ComponentInterface { public: friend class LifecycleComponentPublicInterface; @@ -257,13 +256,13 @@ class LifecycleComponent : public ComponentInterface::create_output; - using ComponentInterface::inputs_; - using ComponentInterface::outputs_; - using ComponentInterface::qos_; - using ComponentInterface::publish_predicates; - using ComponentInterface::publish_outputs; - using ComponentInterface::evaluate_periodic_callbacks; + using ComponentInterface::create_output; + using ComponentInterface::inputs_; + using ComponentInterface::outputs_; + using ComponentInterface::periodic_outputs_; + using ComponentInterface::publish_predicates; + using ComponentInterface::publish_outputs; + using ComponentInterface::evaluate_periodic_callbacks; }; template @@ -273,13 +272,14 @@ inline void LifecycleComponent::add_output( ) { if (this->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED && this->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, - "Adding output in state " << this->get_current_state().label() << " is not allowed."); + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Adding output in state " << this->get_current_state().label() << " is not allowed."); return; } try { - this->create_output(signal_name, data, default_topic, fixed_topic, publish_on_step); + this->create_output( + modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, signal_name, data, default_topic, fixed_topic, + publish_on_step); } catch (const exceptions::AddSignalException& ex) { RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what()); } diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp index 23ea7bea7..61a08fb17 100644 --- a/source/modulo_components/src/Component.cpp +++ b/source/modulo_components/src/Component.cpp @@ -1,11 +1,19 @@ #include "modulo_components/Component.hpp" using namespace modulo_core::communication; +using namespace rclcpp; namespace modulo_components { -Component::Component(const rclcpp::NodeOptions& node_options, const std::string& fallback_name) : - ComponentInterface(node_options, PublisherType::PUBLISHER, fallback_name), started_(false) { +Component::Component(const NodeOptions& node_options, const std::string& fallback_name) + : Node( + utilities::parse_node_name(node_options, fallback_name), utilities::modify_parameter_overrides(node_options)), + ComponentInterface(std::make_shared>( + 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(), + Node::get_node_services_interface(), Node::get_node_time_source_interface(), + Node::get_node_timers_interface(), Node::get_node_topics_interface(), Node::get_node_waitables_interface())), + started_(false) { this->add_predicate("is_finished", false); } diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index ba2ab32b0..bbb119d94 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -3,12 +3,19 @@ #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) : - ComponentInterface( - node_options, PublisherType::LIFECYCLE_PUBLISHER, fallback_name) { +LifecycleComponent::LifecycleComponent(const rclcpp::NodeOptions& node_options, const std::string& fallback_name) + : LifecycleNode( + utilities::parse_node_name(node_options, fallback_name), utilities::modify_parameter_overrides(node_options)), + ComponentInterface(std::make_shared>( + LifecycleNode::get_node_base_interface(), LifecycleNode::get_node_clock_interface(), + LifecycleNode::get_node_graph_interface(), LifecycleNode::get_node_logging_interface(), + LifecycleNode::get_node_parameters_interface(), LifecycleNode::get_node_services_interface(), + LifecycleNode::get_node_time_source_interface(), LifecycleNode::get_node_timers_interface(), + LifecycleNode::get_node_topics_interface(), LifecycleNode::get_node_waitables_interface())) { this->add_predicate( "is_unconfigured", [this] { return this->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; @@ -42,25 +49,24 @@ void LifecycleComponent::step() { } } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleComponent::on_configure(const rclcpp_lifecycle::State& previous_state) { +node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_configure(const State& previous_state) { RCLCPP_DEBUG(this->get_logger(), "on_configure called from previous state %s", previous_state.label().c_str()); if (previous_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { RCLCPP_WARN(get_logger(), "Invalid transition 'configure' from state %s.", previous_state.label().c_str()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } if (!this->handle_configure()) { RCLCPP_WARN(get_logger(), "Configuration failed! Reverting to the unconfigured state."); // perform cleanup actions to ensure the component is unconfigured if (this->handle_cleanup()) { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } else { RCLCPP_ERROR(get_logger(), "Could not revert to the unconfigured state! Entering into the error processing transition state."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } bool LifecycleComponent::handle_configure() { @@ -72,18 +78,17 @@ bool LifecycleComponent::on_configure_callback() { return true; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleComponent::on_cleanup(const rclcpp_lifecycle::State& previous_state) { +node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_cleanup(const State& previous_state) { RCLCPP_DEBUG(this->get_logger(), "on_cleanup called from previous state %s", previous_state.label().c_str()); if (previous_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_WARN(get_logger(), "Invalid transition 'cleanup' from state %s.", previous_state.label().c_str()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } if (!this->handle_cleanup()) { RCLCPP_ERROR(get_logger(), "Cleanup failed! Entering into the error processing transition state."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } bool LifecycleComponent::handle_cleanup() { @@ -94,25 +99,24 @@ bool LifecycleComponent::on_cleanup_callback() { return true; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleComponent::on_activate(const rclcpp_lifecycle::State& previous_state) { +node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_activate(const State& previous_state) { RCLCPP_DEBUG(this->get_logger(), "on_activate called from previous state %s", previous_state.label().c_str()); if (previous_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_WARN(get_logger(), "Invalid transition 'activate' from state %s.", previous_state.label().c_str()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } if (!this->handle_activate()) { RCLCPP_WARN(get_logger(), "Activation failed! Reverting to the inactive state."); // perform deactivation actions to ensure the component is inactive if (this->handle_deactivate()) { - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } else { RCLCPP_ERROR(get_logger(), "Could not revert to the inactive state! Entering into the error processing transition state."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } bool LifecycleComponent::handle_activate() { @@ -124,18 +128,17 @@ bool LifecycleComponent::on_activate_callback() { return true; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleComponent::on_deactivate(const rclcpp_lifecycle::State& previous_state) { +node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_deactivate(const State& previous_state) { RCLCPP_DEBUG(this->get_logger(), "on_deactivate called from previous state %s", previous_state.label().c_str()); if (previous_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { RCLCPP_WARN(get_logger(), "Invalid transition 'deactivate' from state %s.", previous_state.label().c_str()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } if (!this->handle_deactivate()) { RCLCPP_ERROR(get_logger(), "Deactivation failed! Entering into the error processing transition state."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } bool LifecycleComponent::handle_deactivate() { @@ -147,12 +150,11 @@ bool LifecycleComponent::on_deactivate_callback() { return true; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleComponent::on_shutdown(const rclcpp_lifecycle::State& previous_state) { +node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_shutdown(const State& previous_state) { RCLCPP_DEBUG(this->get_logger(), "on_shutdown called from previous state %s", previous_state.label().c_str()); switch (previous_state.id()) { case lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED: - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE: if (!this->handle_deactivate()) { RCLCPP_DEBUG(get_logger(), "Shutdown failed during intermediate deactivation!"); @@ -174,13 +176,13 @@ LifecycleComponent::on_shutdown(const rclcpp_lifecycle::State& previous_state) { // this->daemons_.clear(); // this->parameters_.clear(); // this->shutdown_ = true; - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; default: RCLCPP_WARN(get_logger(), "Invalid transition 'shutdown' from state %s.", previous_state.label().c_str()); break; } RCLCPP_ERROR(get_logger(), "Entering into the error processing transition state."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } bool LifecycleComponent::handle_shutdown() { @@ -192,8 +194,7 @@ bool LifecycleComponent::on_shutdown_callback() { return true; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleComponent::on_error(const rclcpp_lifecycle::State& previous_state) { +node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_error(const State& previous_state) { RCLCPP_DEBUG(this->get_logger(), "on_error called from previous state %s", previous_state.label().c_str()); this->set_predicate("in_error_state", true); bool error_handled; @@ -206,10 +207,10 @@ LifecycleComponent::on_error(const rclcpp_lifecycle::State& previous_state) { if (!error_handled) { RCLCPP_ERROR(get_logger(), "Error processing failed! Entering into the finalized state."); // TODO: reset and finalize all needed properties - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } this->set_predicate("in_error_state", false); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } bool LifecycleComponent::handle_error() { @@ -230,52 +231,51 @@ bool LifecycleComponent::configure_outputs() { auto message_pair = interface->get_message_pair(); switch (message_pair->get_type()) { case MessageType::BOOL: { - auto publisher = this->create_publisher(topic_name, this->qos_); + auto publisher = this->create_publisher(topic_name, this->get_qos()); interface = std::make_shared< PublisherHandler< - rclcpp_lifecycle::LifecyclePublisher, std_msgs::msg::Bool>>( + LifecyclePublisher, std_msgs::msg::Bool>>( PublisherType::LIFECYCLE_PUBLISHER, publisher)->create_publisher_interface(message_pair); break; } case MessageType::FLOAT64: { - auto publisher = this->create_publisher(topic_name, this->qos_); + auto publisher = this->create_publisher(topic_name, this->get_qos()); interface = std::make_shared< PublisherHandler< - rclcpp_lifecycle::LifecyclePublisher, std_msgs::msg::Float64>>( + LifecyclePublisher, std_msgs::msg::Float64>>( PublisherType::LIFECYCLE_PUBLISHER, publisher)->create_publisher_interface(message_pair); break; } case MessageType::FLOAT64_MULTI_ARRAY: { auto publisher = this->create_publisher( - topic_name, this->qos_); + topic_name, this->get_qos()); interface = std::make_shared< PublisherHandler< - rclcpp_lifecycle::LifecyclePublisher, - std_msgs::msg::Float64MultiArray>>( + LifecyclePublisher, std_msgs::msg::Float64MultiArray>>( PublisherType::LIFECYCLE_PUBLISHER, publisher)->create_publisher_interface(message_pair); break; } case MessageType::INT32: { - auto publisher = this->create_publisher(topic_name, this->qos_); + auto publisher = this->create_publisher(topic_name, this->get_qos()); interface = std::make_shared< PublisherHandler< - rclcpp_lifecycle::LifecyclePublisher, std_msgs::msg::Int32>>( + LifecyclePublisher, std_msgs::msg::Int32>>( PublisherType::LIFECYCLE_PUBLISHER, publisher)->create_publisher_interface(message_pair); break; } case MessageType::STRING: { - auto publisher = this->create_publisher(topic_name, this->qos_); + auto publisher = this->create_publisher(topic_name, this->get_qos()); interface = std::make_shared< PublisherHandler< - rclcpp_lifecycle::LifecyclePublisher, std_msgs::msg::String>>( + LifecyclePublisher, std_msgs::msg::String>>( PublisherType::LIFECYCLE_PUBLISHER, publisher)->create_publisher_interface(message_pair); break; } case MessageType::ENCODED_STATE: { - auto publisher = this->create_publisher(topic_name, this->qos_); + auto publisher = this->create_publisher(topic_name, this->get_qos()); interface = std::make_shared< PublisherHandler< - rclcpp_lifecycle::LifecyclePublisher, modulo_core::EncodedState>>( + LifecyclePublisher, modulo_core::EncodedState>>( PublisherType::LIFECYCLE_PUBLISHER, publisher)->create_publisher_interface(message_pair); break; } diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 8f9d912a6..25f060406 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -85,6 +85,7 @@ class ComponentPublicInterface : public Component { explicit ComponentPublicInterface( const rclcpp::NodeOptions& node_options, const std::string& fallback_name = "ComponentPublicInterface" ) : Component(node_options, fallback_name) {} + using ComponentInterface::get_parameter_value; using Component::add_output; using Component::outputs_; using Component::periodic_outputs_; @@ -96,6 +97,7 @@ class LifecycleComponentPublicInterface : public LifecycleComponent { public: explicit LifecycleComponentPublicInterface(const rclcpp::NodeOptions& node_options) : LifecycleComponent(node_options) {} + using ComponentInterface::get_parameter_value; using LifecycleComponent::add_output; using LifecycleComponent::configure_outputs; using LifecycleComponent::activate_outputs; diff --git a/source/modulo_components/test/cpp/test_component.cpp b/source/modulo_components/test/cpp/test_component.cpp index 5ac7f2e5f..2a6a092fb 100644 --- a/source/modulo_components/test/cpp/test_component.cpp +++ b/source/modulo_components/test/cpp/test_component.cpp @@ -26,6 +26,30 @@ class ComponentTest : public ::testing::Test { std::shared_ptr component_; }; +TEST_F(ComponentTest, RatePeriodParameters) { + std::shared_ptr component; + auto node_options = rclcpp::NodeOptions(); + component = std::make_shared(node_options); + EXPECT_EQ(component->template get_parameter_value("rate"), 10); + EXPECT_EQ(component->template get_parameter_value("period"), 0.1); + + node_options = rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("rate", 200)}); + component = std::make_shared(node_options); + EXPECT_EQ(component->template get_parameter_value("rate"), 200); + EXPECT_EQ(component->template get_parameter_value("period"), 0.005); + + node_options = rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("period", 0.01)}); + component = std::make_shared(node_options); + EXPECT_EQ(component->template get_parameter_value("rate"), 100); + EXPECT_EQ(component->template get_parameter_value("period"), 0.01); + + node_options = + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("rate", 200), rclcpp::Parameter("period", 0.01)}); + component = std::make_shared(node_options); + EXPECT_EQ(component->template get_parameter_value("rate"), 200); + EXPECT_EQ(component->template get_parameter_value("period"), 0.005); +} + TEST_F(ComponentTest, AddRemoveOutput) { std::shared_ptr data = make_shared_state(CartesianState::Random("test")); component_->add_output("_tEsT_#1@3", data); diff --git a/source/modulo_components/test/cpp/test_lifecycle_component.cpp b/source/modulo_components/test/cpp/test_lifecycle_component.cpp index f0b0f353c..446810305 100644 --- a/source/modulo_components/test/cpp/test_lifecycle_component.cpp +++ b/source/modulo_components/test/cpp/test_lifecycle_component.cpp @@ -26,6 +26,30 @@ class LifecycleComponentTest : public ::testing::Test { std::shared_ptr component_; }; +TEST_F(LifecycleComponentTest, RatePeriodParameters) { + std::shared_ptr component; + auto node_options = rclcpp::NodeOptions(); + component = std::make_shared(node_options); + EXPECT_EQ(component->template get_parameter_value("rate"), 10); + EXPECT_EQ(component->template get_parameter_value("period"), 0.1); + + node_options = rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("rate", 200)}); + component = std::make_shared(node_options); + EXPECT_EQ(component->template get_parameter_value("rate"), 200); + EXPECT_EQ(component->template get_parameter_value("period"), 0.005); + + node_options = rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("period", 0.01)}); + component = std::make_shared(node_options); + EXPECT_EQ(component->template get_parameter_value("rate"), 100); + EXPECT_EQ(component->template get_parameter_value("period"), 0.01); + + node_options = + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("rate", 200), rclcpp::Parameter("period", 0.01)}); + component = std::make_shared(node_options); + EXPECT_EQ(component->template get_parameter_value("rate"), 200); + EXPECT_EQ(component->template get_parameter_value("period"), 0.005); +} + TEST_F(LifecycleComponentTest, AddRemoveOutput) { std::shared_ptr data = make_shared_state(CartesianState::Random("test")); component_->add_output("test", data); From 9fddd2df59b4f0a739b0c2afdc135b57bc054e30 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Tue, 24 Oct 2023 17:33:10 +0200 Subject: [PATCH 3/5] chore: revise component interface by moving implementations to source (#39) --- CHANGELOG.md | 1 + .../include/modulo_components/Component.hpp | 1 + .../modulo_components/ComponentInterface.hpp | 753 +++--------------- .../modulo_components/LifecycleComponent.hpp | 1 + .../src/ComponentInterface.cpp | 520 +++++++++++- .../test/cpp/test_component_interface.cpp | 1 + 6 files changed, 636 insertions(+), 641 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 233d10c2d..bacdef683 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ Release Versions: ## Upcoming changes (in development) +- Revise ComponentInterface by moving implementations to source file (#39) - Update component classes to inherit from new ComponentInterface (#38) - Refactor ComponentInterface to use node interfaces (#33) diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 0836ff07b..2b56251d0 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -80,6 +80,7 @@ class Component : public rclcpp::Node, public ComponentInterface { using ComponentInterface::inputs_; using ComponentInterface::outputs_; using ComponentInterface::periodic_outputs_; + using ComponentInterface::publish_predicate; using ComponentInterface::publish_predicates; using ComponentInterface::publish_outputs; using ComponentInterface::evaluate_periodic_callbacks; diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 10534c38a..63231e9e5 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -2,8 +2,6 @@ #include -#include -#include #include #include #include @@ -14,19 +12,14 @@ #include #include #include -#include -#include -#include #include #include #include #include -#include "modulo_components/exceptions/AddServiceException.hpp" #include "modulo_components/exceptions/AddSignalException.hpp" #include "modulo_components/exceptions/ComponentParameterException.hpp" -#include "modulo_components/exceptions/LookupTransformException.hpp" #include "modulo_components/utilities/utilities.hpp" #include "modulo_components/utilities/predicate_variant.hpp" @@ -95,7 +88,7 @@ class ComponentInterface { * @param parameter A ParameterInterface pointer to a Parameter instance * @param description The description of the parameter * @param read_only If true, the value of the parameter cannot be changed after declaration - * @raise ComponentParameterError if the parameter could not be added + * @throws ComponentParameterError if the parameter could not be added */ void add_parameter( const std::shared_ptr& parameter, const std::string& description, @@ -111,7 +104,7 @@ class ComponentInterface { * @param value The value of the parameter * @param description The description of the parameter * @param read_only If true, the value of the parameter cannot be changed after declaration - * @raise ComponentParameterError if the parameter could not be added + * @throws ComponentParameterError if the parameter could not be added */ template void add_parameter(const std::string& name, const T& value, const std::string& description, bool read_only = false); @@ -132,7 +125,7 @@ class ComponentInterface { * @return The value of the parameter */ template - T get_parameter_value(const std::string& name) const; + [[nodiscard]] T get_parameter_value(const std::string& name) const; /** * @brief Set the value of a parameter. @@ -231,18 +224,6 @@ class ComponentInterface { */ void declare_output(const std::string& signal_name, const std::string& default_topic = "", bool fixed_topic = false); - /** - * @brief Remove an input from the map of inputs. - * @param signal_name The name of the input - */ - void remove_input(const std::string& signal_name); - - /** - * @brief Remove an output from the map of outputs. - * @param signal_name The name of the output - */ - void remove_output(const std::string& signal_name); - /** * @brief Add and configure an input signal of the component. * @tparam DataT Type of the data pointer @@ -286,6 +267,43 @@ class ComponentInterface { const std::string& default_topic = "", bool fixed_topic = false ); + /** + * @brief Helper function to parse the signal name and add an unconfigured PublisherInterface to the map of outputs. + * @tparam DataT Type of the data pointer + * @param signal_name Name of the output signal + * @param data Data to transmit on the output signal + * @param default_topic If set, the default value for the topic name to use + * @param fixed_topic If true, the topic name of the output signal is fixed + * @param publish_on_step If true, the output is published periodically on step + * @throws modulo_components::exceptions::AddSignalException if the output could not be created + * (empty name, already registered) + * @return The parsed signal name + */ + template + std::string create_output( + modulo_core::communication::PublisherType publisher_type, const std::string& signal_name, + const std::shared_ptr& data, const std::string& default_topic, bool fixed_topic, bool publish_on_step + ); + + /** + * @brief Trigger the publishing of an output + * @param signal_name The name of the output signal + * @throws ComponentException if the output is being published periodically or if the signal name could not be found + */ + void publish_output(const std::string& signal_name); + + /** + * @brief Remove an input from the map of inputs. + * @param signal_name The name of the input + */ + void remove_input(const std::string& signal_name); + + /** + * @brief Remove an output from the map of outputs. + * @param signal_name The name of the output + */ + void remove_output(const std::string& signal_name); + /** * @brief Add a service to trigger a callback function with no input arguments. * @param service_name The name of the service @@ -329,43 +347,6 @@ class ComponentInterface { */ void add_tf_listener(); - /** - * @brief Helper function to parse the signal name and add an unconfigured PublisherInterface to the map of outputs. - * @tparam DataT Type of the data pointer - * @param signal_name Name of the output signal - * @param data Data to transmit on the output signal - * @param default_topic If set, the default value for the topic name to use - * @param fixed_topic If true, the topic name of the output signal is fixed - * @param publish_on_step If true, the output is published periodically on step - * @throws modulo_components::exceptions::AddSignalException if the output could not be created - * (empty name, already registered) - * @return The parsed signal name - */ - template - std::string create_output( - modulo_core::communication::PublisherType publisher_type, const std::string& signal_name, - const std::shared_ptr& data, const std::string& default_topic, bool fixed_topic, bool publish_on_step - ); - - /** - * @brief Getter of the Quality of Service attribute. - * @return The Quality of Service attribute - */ - [[nodiscard]] rclcpp::QoS get_qos() const; - - /** - * @brief Set the Quality of Service for ROS publishers and subscribers. - * @param qos The desired Quality of Service - */ - void set_qos(const rclcpp::QoS& qos); - - /** - * @brief Trigger the publishing of an output - * @param signal_name The name of the output signal - * @throws ComponentException if the output is being published periodically or if the signal name could not be found - */ - void publish_output(const std::string& signal_name); - /** * @brief Send a transform to TF. * @param transform The transform to send @@ -419,6 +400,23 @@ class ComponentInterface { const std::string& frame, const std::string& reference_frame = "world", double validity_period = -1.0, const tf2::Duration& duration = tf2::Duration(std::chrono::microseconds(10))); + /** + * @brief Getter of the Quality of Service attribute. + * @return The Quality of Service attribute + */ + [[nodiscard]] rclcpp::QoS get_qos() const; + + /** + * @brief Set the Quality of Service for ROS publishers and subscribers. + * @param qos The desired Quality of Service + */ + void set_qos(const rclcpp::QoS& qos); + + /** + * @brief Put the component in error state by setting the 'in_error_state' predicate to true. + */ + virtual void raise_error(); + /** * @brief Helper function to publish a predicate. * @param name The name of the predicate to publish @@ -440,24 +438,6 @@ class ComponentInterface { */ void evaluate_periodic_callbacks(); - /** - * @brief Helper function to send a vector of transforms through a transform broadcaster - * @tparam T The type of the broadcaster (tf2_ros::TransformBroadcaster or tf2_ros::StaticTransformBroadcaster) - * @param transforms The transforms to send - * @param tf_broadcaster A pointer to a configured transform broadcaster object - * @param is_static If true, treat the broadcaster as a static frame broadcaster for the sake of log messages - */ - template - void publish_transforms( - const std::vector& transforms, const std::shared_ptr& tf_broadcaster, - bool is_static = false - ); - - /** - * @brief Put the component in error state by setting the 'in_error_state' predicate to true. - */ - virtual void raise_error(); - std::map> inputs_; ///< Map of inputs std::map> outputs_; ///< Map of outputs std::map periodic_outputs_; ///< Map of outputs with periodic publishing flag @@ -493,19 +473,6 @@ class ComponentInterface { */ void set_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate); - /** - * @brief Remove a signal from the map of inputs or outputs. - * @tparam T The type of the map entry (SubscriptionInterface or PublisherInterface) - * @param signal_name The name of the signal to remove - * @param signal_map The map of signals (either inputs or outputs) - * @param skip_check If true, skip the check if the signal exists in the map and return false otherwise - * @return True if the signal was removed, false if it didn't exist - */ - template - bool remove_signal( - const std::string& signal_name, std::map>& signal_map, bool skip_check = false - ); - /** * @brief Declare a signal to create the topic parameter without adding it to the map of signals. * @param signal_name The name of the signal @@ -519,6 +486,19 @@ class ComponentInterface { const std::string& signal_name, const std::string& type, const std::string& default_topic, bool fixed_topic ); + /** + * @brief Remove a signal from the map of inputs or outputs. + * @tparam T The type of the map entry (SubscriptionInterface or PublisherInterface) + * @param signal_name The name of the signal to remove + * @param signal_map The map of signals (either inputs or outputs) + * @param skip_check If true, skip the check if the signal exists in the map and return false otherwise + * @return True if the signal was removed, false if it didn't exist + */ + template + bool remove_signal( + const std::string& signal_name, std::map>& signal_map, bool skip_check = false + ); + /** * @brief Validate an add_service request by parsing the service name and checking the maps of registered services. * @param service_name The name of the service @@ -528,6 +508,19 @@ class ComponentInterface { */ std::string validate_service_name(const std::string& service_name); + /** + * @brief Helper function to send a vector of transforms through a transform broadcaster + * @tparam T The type of the broadcaster (tf2_ros::TransformBroadcaster or tf2_ros::StaticTransformBroadcaster) + * @param transforms The transforms to send + * @param tf_broadcaster A pointer to a configured transform broadcaster object + * @param is_static If true, treat the broadcaster as a static frame broadcaster for the sake of log messages + */ + template + void publish_transforms( + const std::vector& transforms, const std::shared_ptr& tf_broadcaster, + bool is_static = false + ); + /** * @brief Helper method to look up a transform from TF. * @param frame The desired frame of the transform @@ -578,10 +571,6 @@ class ComponentInterface { bool set_parameter_callback_called_ = false; ///< Flag to indicate if on_set_parameter_callback was called }; -inline void ComponentInterface::step() {} - -inline void ComponentInterface::on_step_callback() {} - template inline void ComponentInterface::add_parameter( const std::string& name, const T& value, const std::string& description, bool read_only @@ -603,58 +592,6 @@ inline T ComponentInterface::get_parameter_value(const std::string& name) const } } -inline void ComponentInterface::add_parameter( - const std::shared_ptr& parameter, const std::string& description, - bool read_only -) { - this->set_parameter_callback_called_ = false; - rclcpp::Parameter ros_param; - try { - ros_param = modulo_core::translators::write_parameter(parameter); - } catch (const modulo_core::exceptions::ParameterTranslationException& ex) { - throw exceptions::ComponentParameterException("Failed to add parameter: " + std::string(ex.what())); - } - if (!this->node_parameters_->has_parameter(parameter->get_name())) { - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding parameter '" << parameter->get_name() << "'."); - this->parameter_map_.set_parameter(parameter); - try { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = description; - descriptor.read_only = read_only; - if (parameter->is_empty()) { - descriptor.dynamic_typing = true; - descriptor.type = modulo_core::translators::get_ros_parameter_type(parameter->get_parameter_type()); - this->node_parameters_->declare_parameter(parameter->get_name(), rclcpp::ParameterValue{}, descriptor); - } else { - this->node_parameters_->declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor); - } - if (!this->set_parameter_callback_called_) { - auto result = - this->on_set_parameters_callback({this->node_parameters_->get_parameters({parameter->get_name()})}); - if (!result.successful) { - this->node_parameters_->undeclare_parameter(parameter->get_name()); - throw exceptions::ComponentParameterException(result.reason); - } - } - } catch (const std::exception& ex) { - this->parameter_map_.remove_parameter(parameter->get_name()); - throw exceptions::ComponentParameterException("Failed to add parameter: " + std::string(ex.what())); - } - } else { - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), - "Parameter '" << parameter->get_name() << "' already exists."); - } -} - -inline std::shared_ptr -ComponentInterface::get_parameter(const std::string& name) const { - try { - return this->parameter_map_.get_parameter(name); - } catch (const state_representation::exceptions::InvalidParameterException& ex) { - throw exceptions::ComponentParameterException("Failed to get parameter '" + name + "': " + ex.what()); - } -} - template inline void ComponentInterface::set_parameter_value(const std::string& name, const T& value) { try { @@ -672,243 +609,6 @@ inline void ComponentInterface::set_parameter_value(const std::string& name, con } } -inline bool ComponentInterface::validate_parameter( - const std::shared_ptr& parameter -) { - if (parameter->get_name() == "rate") { - auto value = parameter->get_parameter_value(); - 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(); - 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); -} - -inline bool ComponentInterface::on_validate_parameter_callback( - const std::shared_ptr& -) { - return true; -} - -inline rcl_interfaces::msg::SetParametersResult -ComponentInterface::on_set_parameters_callback(const std::vector& parameters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - for (const auto& ros_parameter : parameters) { - try { - if (ros_parameter.get_name().substr(0, 27) == "qos_overrides./tf.publisher") { - continue; - } - // get the associated parameter interface by name - auto parameter = parameter_map_.get_parameter(ros_parameter.get_name()); - - // convert the ROS parameter into a ParameterInterface without modifying the original - auto new_parameter = modulo_core::translators::read_parameter_const(ros_parameter, parameter); - if (!this->validate_parameter(new_parameter)) { - result.successful = false; - result.reason += "Validation of parameter '" + ros_parameter.get_name() + "' returned false!"; - } else if (!new_parameter->is_empty()) { - // update the value of the parameter in the map - modulo_core::translators::copy_parameter_value(new_parameter, parameter); - } - } catch (const std::exception& ex) { - result.successful = false; - result.reason += ex.what(); - } - } - this->set_parameter_callback_called_ = true; - return result; -} - -inline void ComponentInterface::add_predicate(const std::string& name, bool predicate) { - this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); -} - -inline void ComponentInterface::add_predicate( - const std::string& name, const std::function& predicate -) { - this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); -} - -inline void ComponentInterface::add_variant_predicate( - const std::string& name, const utilities::PredicateVariant& predicate -) { - if (name.empty()) { - RCLCPP_ERROR(this->node_logging_->get_logger(), "Failed to add predicate: Provide a non empty string as a name."); - return; - } - if (this->predicates_.find(name) != this->predicates_.end()) { - RCLCPP_WARN_STREAM(this->node_logging_->get_logger(), - "Predicate with name '" << name << "' already exists, overwriting."); - } else { - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding predicate '" << name << "'."); - } - this->predicates_.insert_or_assign(name, predicate); -} - -inline bool ComponentInterface::get_predicate(const std::string& predicate_name) { - auto predicate_iterator = this->predicates_.find(predicate_name); - // if there is no predicate with that name simply return false with an error message - if (predicate_iterator == this->predicates_.end()) { - RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, - "Failed to get predicate '" << predicate_name - << "': Predicate does not exists, returning false."); - return false; - } - // try to get the value from the variant as a bool - auto* ptr_value = std::get_if(&predicate_iterator->second); - if (ptr_value) { - return *ptr_value; - } - // if previous check failed, it means the variant is actually a callback function - auto callback_function = std::get>(predicate_iterator->second); - bool value = false; - try { - value = (callback_function)(); - } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, - "Failed to evaluate callback of predicate '" << predicate_name - << "', returning false: " << ex.what()); - } - return value; -} - -inline void ComponentInterface::add_trigger(const std::string& trigger_name) { - if (trigger_name.empty()) { - RCLCPP_ERROR(this->node_logging_->get_logger(), "Failed to add trigger: Provide a non empty string as a name."); - return; - } - if (this->triggers_.find(trigger_name) != this->triggers_.end() - || this->predicates_.find(trigger_name) != this->predicates_.end()) { - RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), "Failed to add trigger: there is already a trigger or " - "predicate with name '" << trigger_name << "'."); - return; - } - this->triggers_.insert_or_assign(trigger_name, false); - this->add_predicate( - trigger_name, [this, trigger_name] { - auto value = this->triggers_.at(trigger_name); - this->triggers_.at(trigger_name) = false; - return value; - }); -} - -inline void ComponentInterface::trigger(const std::string& trigger_name) { - if (this->triggers_.find(trigger_name) == this->triggers_.end()) { - RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), "Failed to trigger: could not find trigger" - " with name '" << trigger_name << "'."); - return; - } - this->triggers_.at(trigger_name) = true; - publish_predicate(trigger_name); -} - -inline void ComponentInterface::set_variant_predicate( - const std::string& name, const utilities::PredicateVariant& predicate -) { - auto predicate_iterator = this->predicates_.find(name); - if (predicate_iterator == this->predicates_.end()) { - RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, - "Failed to set predicate '" << name << "': Predicate does not exist."); - return; - } - predicate_iterator->second = predicate; - this->publish_predicate(name); -} - -inline void ComponentInterface::set_predicate(const std::string& name, bool predicate) { - this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); -} - -inline void ComponentInterface::set_predicate( - const std::string& name, const std::function& predicate -) { - this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); -} - -template -inline bool ComponentInterface::remove_signal( - const std::string& signal_name, std::map>& signal_map, bool skip_check -) { - if (!skip_check && signal_map.find(signal_name) == signal_map.cend()) { - return false; - } else { - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Removing signal '" << signal_name << "'."); - signal_map.at(signal_name).reset(); - return signal_map.erase(signal_name); - } -} - -inline void ComponentInterface::remove_input(const std::string& signal_name) { - if (!this->template remove_signal(signal_name, this->inputs_)) { - auto parsed_signal_name = utilities::parse_topic_name(signal_name); - if (!this->template remove_signal(parsed_signal_name, this->inputs_)) { - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), - "Unknown input '" << signal_name << "' (parsed name was '" << parsed_signal_name << "')."); - } - } -} - -inline void ComponentInterface::remove_output(const std::string& signal_name) { - if (!this->template remove_signal(signal_name, this->outputs_)) { - auto parsed_signal_name = utilities::parse_topic_name(signal_name); - if (!this->template remove_signal(parsed_signal_name, this->outputs_)) { - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), - "Unknown output '" << signal_name << "' (parsed name was '" << parsed_signal_name << "')."); - } - } -} - -inline void ComponentInterface::declare_signal( - const std::string& signal_name, const std::string& type, const std::string& default_topic, bool fixed_topic -) { - std::string parsed_signal_name = utilities::parse_topic_name(signal_name); - if (parsed_signal_name.empty()) { - throw exceptions::AddSignalException( - "The parsed signal name for " + type + " '" + signal_name - + "' is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_])."); - } - if (this->inputs_.find(parsed_signal_name) != this->inputs_.cend()) { - throw exceptions::AddSignalException("Signal with name '" + parsed_signal_name + "' already exists as input."); - } - if (this->outputs_.find(parsed_signal_name) != this->outputs_.cend()) { - throw exceptions::AddSignalException("Signal with name '" + parsed_signal_name + "' already exists as output."); - } - std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; - auto parameter_name = parsed_signal_name + "_topic"; - if (this->node_parameters_->has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { - this->set_parameter_value(parameter_name, topic_name); - } else { - this->add_parameter( - parameter_name, topic_name, "Signal topic name of " + type + " '" + parsed_signal_name + "'", fixed_topic); - } - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), - "Declared signal '" << parsed_signal_name << "' and parameter '" << parameter_name - << "' with value '" << topic_name << "'."); -} - -inline void ComponentInterface::declare_input( - const std::string& signal_name, const std::string& default_topic, bool fixed_topic -) { - this->declare_signal(signal_name, "input", default_topic, fixed_topic); -} - -inline void ComponentInterface::declare_output( - const std::string& signal_name, const std::string& default_topic, bool fixed_topic -) { - this->declare_signal(signal_name, "output", default_topic, fixed_topic); -} - template inline void ComponentInterface::add_input( const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic, @@ -1016,141 +716,47 @@ inline void ComponentInterface::add_input( } } -inline std::string ComponentInterface::validate_service_name(const std::string& service_name) { - std::string parsed_service_name = utilities::parse_topic_name(service_name); - if (parsed_service_name.empty()) { - throw exceptions::AddServiceException( - "The parsed service name for service '" + service_name - + "' is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_])."); - } - if (this->empty_services_.find(parsed_service_name) != this->empty_services_.cend() - || this->string_services_.find(parsed_service_name) != this->string_services_.cend()) { - throw exceptions::AddServiceException("Service with name '" + parsed_service_name + "' already exists."); - } - return parsed_service_name; -} - -inline void ComponentInterface::add_service( - const std::string& service_name, const std::function& callback +template +inline std::string ComponentInterface::create_output( + modulo_core::communication::PublisherType publisher_type, const std::string& signal_name, + const std::shared_ptr& data, const std::string& default_topic, bool fixed_topic, bool publish_on_step ) { + using namespace modulo_core::communication; try { - std::string parsed_service_name = this->validate_service_name(service_name); - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding empty service '" << parsed_service_name << "'."); - auto service = rclcpp::create_service( - this->node_base_, this->node_services_, "~/" + parsed_service_name, [callback]( - const std::shared_ptr, - std::shared_ptr response - ) { - try { - auto callback_response = callback(); - response->success = callback_response.success; - response->message = callback_response.message; - } catch (const std::exception& ex) { - response->success = false; - response->message = ex.what(); - } - }, this->qos_, this->cb_group_); - this->empty_services_.insert_or_assign(parsed_service_name, service); + auto parsed_signal_name = utilities::parse_topic_name(signal_name); + if (data == nullptr) { + throw modulo_core::exceptions::NullPointerException( + "Invalid data pointer for output '" + parsed_signal_name + "'."); + } + this->declare_output(parsed_signal_name, default_topic, fixed_topic); + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), + "Creating output '" << parsed_signal_name << "' (provided signal name was '" << signal_name + << "')."); + auto message_pair = make_shared_message_pair(data, this->node_clock_->get_clock()); + this->outputs_.insert_or_assign( + parsed_signal_name, std::make_shared(publisher_type, message_pair)); + this->periodic_outputs_.insert_or_assign(parsed_signal_name, publish_on_step); + return parsed_signal_name; + } catch (const exceptions::AddSignalException&) { + throw; } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), - "Failed to add service '" << service_name << "': " << ex.what()); + throw exceptions::AddSignalException(ex.what()); } } -inline void ComponentInterface::add_service( - const std::string& service_name, const std::function& callback +template +inline bool ComponentInterface::remove_signal( + const std::string& signal_name, std::map>& signal_map, bool skip_check ) { - try { - std::string parsed_service_name = this->validate_service_name(service_name); - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding string service '" << parsed_service_name << "'."); - auto service = rclcpp::create_service( - this->node_base_, this->node_services_, "~/" + parsed_service_name, [callback]( - const std::shared_ptr request, - std::shared_ptr response - ) { - try { - auto callback_response = callback(request->payload); - response->success = callback_response.success; - response->message = callback_response.message; - } catch (const std::exception& ex) { - response->success = false; - response->message = ex.what(); - } - }, this->qos_, this->cb_group_); - this->string_services_.insert_or_assign(parsed_service_name, service); - } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), - "Failed to add service '" << service_name << "': " << ex.what()); - } -} - -inline void ComponentInterface::add_periodic_callback(const std::string& name, const std::function& callback) { - if (name.empty()) { - RCLCPP_ERROR(this->node_logging_->get_logger(), - "Failed to add periodic function: Provide a non empty string as a name."); - return; - } - if (this->periodic_callbacks_.find(name) != this->periodic_callbacks_.end()) { - RCLCPP_WARN_STREAM(this->node_logging_->get_logger(), - "Periodic function '" << name << "' already exists, overwriting."); - } else { - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding periodic function '" << name << "'."); - } - this->periodic_callbacks_.template insert_or_assign(name, callback); -} - -inline void ComponentInterface::add_tf_broadcaster() { - if (this->tf_broadcaster_ == nullptr) { - RCLCPP_DEBUG(this->node_logging_->get_logger(), "Adding TF broadcaster."); - console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); - this->tf_broadcaster_ = std::make_shared( - this->node_parameters_, this->node_topics_, tf2_ros::DynamicBroadcasterQoS()); - } else { - RCLCPP_DEBUG(this->node_logging_->get_logger(), "TF broadcaster already exists."); - } -} - -inline void ComponentInterface::add_static_tf_broadcaster() { - if (this->static_tf_broadcaster_ == nullptr) { - RCLCPP_DEBUG(this->node_logging_->get_logger(), "Adding static TF broadcaster."); - console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); - tf2_ros::StaticBroadcasterQoS qos; - rclcpp::PublisherOptionsWithAllocator> options; - this->static_tf_broadcaster_ = - std::make_shared(this->node_parameters_, this->node_topics_, qos, options); - } else { - RCLCPP_DEBUG(this->node_logging_->get_logger(), "Static TF broadcaster already exists."); - } -} - -inline void ComponentInterface::add_tf_listener() { - if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { - RCLCPP_DEBUG(this->node_logging_->get_logger(), "Adding TF buffer and listener."); - console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); - this->tf_buffer_ = std::make_shared(this->node_clock_->get_clock()); - this->tf_listener_ = std::make_shared(*this->tf_buffer_); + if (!skip_check && signal_map.find(signal_name) == signal_map.cend()) { + return false; } else { - RCLCPP_DEBUG(this->node_logging_->get_logger(), "TF buffer and listener already exist."); + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Removing signal '" << signal_name << "'."); + signal_map.at(signal_name).reset(); + return signal_map.erase(signal_name); } } -inline void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { - this->send_transforms(std::vector{transform}); -} - -inline void ComponentInterface::send_transforms(const std::vector& transforms) { - this->template publish_transforms(transforms, this->tf_broadcaster_); -} - -inline void ComponentInterface::send_static_transform(const state_representation::CartesianPose& transform) { - this->send_static_transforms(std::vector{transform}); -} - -inline void -ComponentInterface::send_static_transforms(const std::vector& transforms) { - this->template publish_transforms(transforms, this->static_tf_broadcaster_, true); -} - template inline void ComponentInterface::publish_transforms( const std::vector& transforms, const std::shared_ptr& tf_broadcaster, @@ -1178,137 +784,4 @@ inline void ComponentInterface::publish_transforms( "Failed to send " << modifier << "transform: " << ex.what()); } } - -inline geometry_msgs::msg::TransformStamped ComponentInterface::lookup_ros_transform( - const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point, - const tf2::Duration& duration -) { - if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { - throw exceptions::LookupTransformException("Failed to lookup transform: To TF buffer / listener configured."); - } - try { - return this->tf_buffer_->lookupTransform(reference_frame, frame, time_point, duration);; - } catch (const tf2::TransformException& ex) { - throw exceptions::LookupTransformException(std::string("Failed to lookup transform: ").append(ex.what())); - } -} - -inline state_representation::CartesianPose ComponentInterface::lookup_transform( - const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point, - const tf2::Duration& duration -) { - auto transform = this->lookup_ros_transform(frame, reference_frame, time_point, duration); - state_representation::CartesianPose result(frame, reference_frame); - modulo_core::translators::read_message(result, transform); - return result; -} - -inline state_representation::CartesianPose ComponentInterface::lookup_transform( - const std::string& frame, const std::string& reference_frame, double validity_period, const tf2::Duration& duration -) { - auto transform = - this->lookup_ros_transform(frame, reference_frame, tf2::TimePoint(std::chrono::microseconds(0)), duration); - if (validity_period > 0.0 - && (this->node_clock_->get_clock()->now() - transform.header.stamp).seconds() > validity_period) { - throw exceptions::LookupTransformException("Failed to lookup transform: Latest transform is too old!"); - } - state_representation::CartesianPose result(frame, reference_frame); - modulo_core::translators::read_message(result, transform); - return result; -} - -inline void ComponentInterface::publish_predicate(const std::string& name) { - modulo_component_interfaces::msg::Predicate message; - message.component = this->node_base_->get_fully_qualified_name(); - message.predicate = name; - message.value = this->get_predicate(name); - this->predicate_publisher_->publish(message); -} - -inline void ComponentInterface::publish_predicates() { - for (const auto& predicate : this->predicates_) { - this->publish_predicate(predicate.first); - } -} - -inline void ComponentInterface::publish_output(const std::string& signal_name) { - auto parsed_signal_name = utilities::parse_topic_name(signal_name); - if (this->outputs_.find(parsed_signal_name) == this->outputs_.cend()) { - throw exceptions::ComponentException("Output with name '" + signal_name + "' doesn't exist."); - } - if (this->periodic_outputs_.at(parsed_signal_name)) { - throw exceptions::ComponentException("An output that is published periodically cannot be triggered manually."); - } - try { - this->outputs_.at(parsed_signal_name)->publish(); - } catch (const modulo_core::exceptions::CoreException& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, - "Failed to publish output '" << parsed_signal_name << "': " << ex.what()); - } -} - -inline void ComponentInterface::publish_outputs() { - for (const auto& [signal, publisher] : this->outputs_) { - try { - if (this->periodic_outputs_.at(signal)) { - publisher->publish(); - } - } catch (const modulo_core::exceptions::CoreException& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, - "Failed to publish output '" << signal << "': " << ex.what()); - } - } -} - -inline void ComponentInterface::evaluate_periodic_callbacks() { - for (const auto& [name, callback] : this->periodic_callbacks_) { - try { - callback(); - } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, - "Failed to evaluate periodic function callback '" << name << "': " << ex.what()); - } - } -} - -template -inline std::string ComponentInterface::create_output( - modulo_core::communication::PublisherType publisher_type, const std::string& signal_name, - const std::shared_ptr& data, const std::string& default_topic, bool fixed_topic, bool publish_on_step -) { - using namespace modulo_core::communication; - try { - auto parsed_signal_name = utilities::parse_topic_name(signal_name); - if (data == nullptr) { - throw modulo_core::exceptions::NullPointerException( - "Invalid data pointer for output '" + parsed_signal_name + "'."); - } - this->declare_output(parsed_signal_name, default_topic, fixed_topic); - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), - "Creating output '" << parsed_signal_name << "' (provided signal name was '" << signal_name - << "')."); - auto message_pair = make_shared_message_pair(data, this->node_clock_->get_clock()); - this->outputs_.insert_or_assign( - parsed_signal_name, std::make_shared(publisher_type, message_pair)); - this->periodic_outputs_.insert_or_assign(parsed_signal_name, publish_on_step); - return parsed_signal_name; - } catch (const exceptions::AddSignalException&) { - throw; - } catch (const std::exception& ex) { - throw exceptions::AddSignalException(ex.what()); - } -} - -inline void ComponentInterface::raise_error() { - RCLCPP_DEBUG(this->node_logging_->get_logger(), "raise_error called: Setting predicate 'in_error_state' to true."); - this->set_predicate("in_error_state", true); -} - -inline rclcpp::QoS ComponentInterface::get_qos() const { - return this->qos_; -} - -inline void ComponentInterface::set_qos(const rclcpp::QoS& qos) { - this->qos_ = qos; -} }// namespace modulo_components diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 441f5ecd0..84d3539f4 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -260,6 +260,7 @@ class LifecycleComponent : public rclcpp_lifecycle::LifecycleNode, public Compon using ComponentInterface::inputs_; using ComponentInterface::outputs_; using ComponentInterface::periodic_outputs_; + using ComponentInterface::publish_predicate; using ComponentInterface::publish_predicates; using ComponentInterface::publish_outputs; using ComponentInterface::evaluate_periodic_callbacks; diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index 8c7cfd310..24cea1cbc 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -1,6 +1,14 @@ #include "modulo_components/ComponentInterface.hpp" -using namespace modulo_core::communication; +#include +#include + +#include +#include +#include + +#include "modulo_components/exceptions/AddServiceException.hpp" +#include "modulo_components/exceptions/LookupTransformException.hpp" namespace modulo_components { @@ -32,4 +40,514 @@ ComponentInterface::ComponentInterface( std::chrono::nanoseconds(static_cast(this->get_parameter_value("period") * 1e9)), [this] { this->step(); }, this->cb_group_, this->node_base_.get(), this->node_timers_.get()); } + +void ComponentInterface::step() {} + +void ComponentInterface::on_step_callback() {} + +void ComponentInterface::add_parameter( + const std::shared_ptr& parameter, const std::string& description, + bool read_only +) { + this->set_parameter_callback_called_ = false; + rclcpp::Parameter ros_param; + try { + ros_param = modulo_core::translators::write_parameter(parameter); + } catch (const modulo_core::exceptions::ParameterTranslationException& ex) { + throw exceptions::ComponentParameterException("Failed to add parameter: " + std::string(ex.what())); + } + if (!this->node_parameters_->has_parameter(parameter->get_name())) { + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding parameter '" << parameter->get_name() << "'."); + this->parameter_map_.set_parameter(parameter); + try { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = description; + descriptor.read_only = read_only; + if (parameter->is_empty()) { + descriptor.dynamic_typing = true; + descriptor.type = modulo_core::translators::get_ros_parameter_type(parameter->get_parameter_type()); + this->node_parameters_->declare_parameter(parameter->get_name(), rclcpp::ParameterValue{}, descriptor); + } else { + this->node_parameters_->declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor); + } + if (!this->set_parameter_callback_called_) { + auto result = + this->on_set_parameters_callback({this->node_parameters_->get_parameters({parameter->get_name()})}); + if (!result.successful) { + this->node_parameters_->undeclare_parameter(parameter->get_name()); + throw exceptions::ComponentParameterException(result.reason); + } + } + } catch (const std::exception& ex) { + this->parameter_map_.remove_parameter(parameter->get_name()); + throw exceptions::ComponentParameterException("Failed to add parameter: " + std::string(ex.what())); + } + } else { + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), + "Parameter '" << parameter->get_name() << "' already exists."); + } +} + +std::shared_ptr +ComponentInterface::get_parameter(const std::string& name) const { + try { + return this->parameter_map_.get_parameter(name); + } catch (const state_representation::exceptions::InvalidParameterException& ex) { + throw exceptions::ComponentParameterException("Failed to get parameter '" + name + "': " + ex.what()); + } +} + +rcl_interfaces::msg::SetParametersResult +ComponentInterface::on_set_parameters_callback(const std::vector& parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto& ros_parameter : parameters) { + try { + if (ros_parameter.get_name().substr(0, 27) == "qos_overrides./tf.publisher") { + continue; + } + // get the associated parameter interface by name + auto parameter = parameter_map_.get_parameter(ros_parameter.get_name()); + + // convert the ROS parameter into a ParameterInterface without modifying the original + auto new_parameter = modulo_core::translators::read_parameter_const(ros_parameter, parameter); + if (!this->validate_parameter(new_parameter)) { + result.successful = false; + result.reason += "Validation of parameter '" + ros_parameter.get_name() + "' returned false!"; + } else if (!new_parameter->is_empty()) { + // update the value of the parameter in the map + modulo_core::translators::copy_parameter_value(new_parameter, parameter); + } + } catch (const std::exception& ex) { + result.successful = false; + result.reason += ex.what(); + } + } + this->set_parameter_callback_called_ = true; + return result; +} + +bool +ComponentInterface::validate_parameter(const std::shared_ptr& parameter) { + if (parameter->get_name() == "rate") { + auto value = parameter->get_parameter_value(); + 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(); + 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); +} + +bool +ComponentInterface::on_validate_parameter_callback(const std::shared_ptr&) { + return true; +} + +void ComponentInterface::add_predicate(const std::string& name, bool predicate) { + this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); +} + +void ComponentInterface::add_predicate(const std::string& name, const std::function& predicate) { + this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); +} + +void ComponentInterface::add_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate) { + if (name.empty()) { + RCLCPP_ERROR(this->node_logging_->get_logger(), "Failed to add predicate: Provide a non empty string as a name."); + return; + } + if (this->predicates_.find(name) != this->predicates_.end()) { + RCLCPP_WARN_STREAM(this->node_logging_->get_logger(), + "Predicate with name '" << name << "' already exists, overwriting."); + } else { + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding predicate '" << name << "'."); + } + this->predicates_.insert_or_assign(name, predicate); +} + +bool ComponentInterface::get_predicate(const std::string& predicate_name) { + auto predicate_iterator = this->predicates_.find(predicate_name); + // if there is no predicate with that name simply return false with an error message + if (predicate_iterator == this->predicates_.end()) { + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, + "Failed to get predicate '" << predicate_name + << "': Predicate does not exists, returning false."); + return false; + } + // try to get the value from the variant as a bool + auto* ptr_value = std::get_if(&predicate_iterator->second); + if (ptr_value) { + return *ptr_value; + } + // if previous check failed, it means the variant is actually a callback function + auto callback_function = std::get>(predicate_iterator->second); + bool value = false; + try { + value = (callback_function)(); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, + "Failed to evaluate callback of predicate '" << predicate_name + << "', returning false: " << ex.what()); + } + return value; +} + +void ComponentInterface::set_predicate(const std::string& name, bool predicate) { + this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); +} + +void ComponentInterface::set_predicate( + const std::string& name, const std::function& predicate +) { + this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); +} + +void ComponentInterface::set_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate) { + auto predicate_iterator = this->predicates_.find(name); + if (predicate_iterator == this->predicates_.end()) { + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, + "Failed to set predicate '" << name << "': Predicate does not exist."); + return; + } + predicate_iterator->second = predicate; + this->publish_predicate(name); +} + +void ComponentInterface::add_trigger(const std::string& trigger_name) { + if (trigger_name.empty()) { + RCLCPP_ERROR(this->node_logging_->get_logger(), "Failed to add trigger: Provide a non empty string as a name."); + return; + } + if (this->triggers_.find(trigger_name) != this->triggers_.end() + || this->predicates_.find(trigger_name) != this->predicates_.end()) { + RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), "Failed to add trigger: there is already a trigger or " + "predicate with name '" << trigger_name << "'."); + return; + } + this->triggers_.insert_or_assign(trigger_name, false); + this->add_predicate( + trigger_name, [this, trigger_name] { + auto value = this->triggers_.at(trigger_name); + this->triggers_.at(trigger_name) = false; + return value; + }); +} + +void ComponentInterface::trigger(const std::string& trigger_name) { + if (this->triggers_.find(trigger_name) == this->triggers_.end()) { + RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), "Failed to trigger: could not find trigger" + " with name '" << trigger_name << "'."); + return; + } + this->triggers_.at(trigger_name) = true; + publish_predicate(trigger_name); +} + +void ComponentInterface::declare_input( + const std::string& signal_name, const std::string& default_topic, bool fixed_topic +) { + this->declare_signal(signal_name, "input", default_topic, fixed_topic); +} + +void ComponentInterface::declare_output( + const std::string& signal_name, const std::string& default_topic, bool fixed_topic +) { + this->declare_signal(signal_name, "output", default_topic, fixed_topic); +} + +void ComponentInterface::declare_signal( + const std::string& signal_name, const std::string& type, const std::string& default_topic, bool fixed_topic +) { + std::string parsed_signal_name = utilities::parse_topic_name(signal_name); + if (parsed_signal_name.empty()) { + throw exceptions::AddSignalException( + "The parsed signal name for " + type + " '" + signal_name + + "' is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_])."); + } + if (this->inputs_.find(parsed_signal_name) != this->inputs_.cend()) { + throw exceptions::AddSignalException("Signal with name '" + parsed_signal_name + "' already exists as input."); + } + if (this->outputs_.find(parsed_signal_name) != this->outputs_.cend()) { + throw exceptions::AddSignalException("Signal with name '" + parsed_signal_name + "' already exists as output."); + } + std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; + auto parameter_name = parsed_signal_name + "_topic"; + if (this->node_parameters_->has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { + this->set_parameter_value(parameter_name, topic_name); + } else { + this->add_parameter( + parameter_name, topic_name, "Signal topic name of " + type + " '" + parsed_signal_name + "'", fixed_topic); + } + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), + "Declared signal '" << parsed_signal_name << "' and parameter '" << parameter_name + << "' with value '" << topic_name << "'."); +} + +void ComponentInterface::publish_output(const std::string& signal_name) { + auto parsed_signal_name = utilities::parse_topic_name(signal_name); + if (this->outputs_.find(parsed_signal_name) == this->outputs_.cend()) { + throw exceptions::ComponentException("Output with name '" + signal_name + "' doesn't exist."); + } + if (this->periodic_outputs_.at(parsed_signal_name)) { + throw exceptions::ComponentException("An output that is published periodically cannot be triggered manually."); + } + try { + this->outputs_.at(parsed_signal_name)->publish(); + } catch (const modulo_core::exceptions::CoreException& ex) { + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, + "Failed to publish output '" << parsed_signal_name << "': " << ex.what()); + } +} + +void ComponentInterface::remove_input(const std::string& signal_name) { + if (!this->remove_signal(signal_name, this->inputs_)) { + auto parsed_signal_name = utilities::parse_topic_name(signal_name); + if (!this->remove_signal(parsed_signal_name, this->inputs_)) { + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), + "Unknown input '" << signal_name << "' (parsed name was '" << parsed_signal_name << "')."); + } + } +} + +void ComponentInterface::remove_output(const std::string& signal_name) { + if (!this->remove_signal(signal_name, this->outputs_)) { + auto parsed_signal_name = utilities::parse_topic_name(signal_name); + if (!this->remove_signal(parsed_signal_name, this->outputs_)) { + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), + "Unknown output '" << signal_name << "' (parsed name was '" << parsed_signal_name << "')."); + } + } +} + +void ComponentInterface::add_service( + const std::string& service_name, const std::function& callback +) { + try { + std::string parsed_service_name = this->validate_service_name(service_name); + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding empty service '" << parsed_service_name << "'."); + auto service = rclcpp::create_service( + this->node_base_, this->node_services_, "~/" + parsed_service_name, [callback]( + const std::shared_ptr, + std::shared_ptr response + ) { + try { + auto callback_response = callback(); + response->success = callback_response.success; + response->message = callback_response.message; + } catch (const std::exception& ex) { + response->success = false; + response->message = ex.what(); + } + }, this->qos_, this->cb_group_); + this->empty_services_.insert_or_assign(parsed_service_name, service); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), + "Failed to add service '" << service_name << "': " << ex.what()); + } +} + +void ComponentInterface::add_service( + const std::string& service_name, const std::function& callback +) { + try { + std::string parsed_service_name = this->validate_service_name(service_name); + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding string service '" << parsed_service_name << "'."); + auto service = rclcpp::create_service( + this->node_base_, this->node_services_, "~/" + parsed_service_name, [callback]( + const std::shared_ptr request, + std::shared_ptr response + ) { + try { + auto callback_response = callback(request->payload); + response->success = callback_response.success; + response->message = callback_response.message; + } catch (const std::exception& ex) { + response->success = false; + response->message = ex.what(); + } + }, this->qos_, this->cb_group_); + this->string_services_.insert_or_assign(parsed_service_name, service); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), + "Failed to add service '" << service_name << "': " << ex.what()); + } +} + +std::string ComponentInterface::validate_service_name(const std::string& service_name) { + std::string parsed_service_name = utilities::parse_topic_name(service_name); + if (parsed_service_name.empty()) { + throw exceptions::AddServiceException( + "The parsed service name for service '" + service_name + + "' is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_])."); + } + if (this->empty_services_.find(parsed_service_name) != this->empty_services_.cend() + || this->string_services_.find(parsed_service_name) != this->string_services_.cend()) { + throw exceptions::AddServiceException("Service with name '" + parsed_service_name + "' already exists."); + } + return parsed_service_name; +} + +void ComponentInterface::add_periodic_callback(const std::string& name, const std::function& callback) { + if (name.empty()) { + RCLCPP_ERROR(this->node_logging_->get_logger(), + "Failed to add periodic function: Provide a non empty string as a name."); + return; + } + if (this->periodic_callbacks_.find(name) != this->periodic_callbacks_.end()) { + RCLCPP_WARN_STREAM(this->node_logging_->get_logger(), + "Periodic function '" << name << "' already exists, overwriting."); + } else { + RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding periodic function '" << name << "'."); + } + this->periodic_callbacks_.insert_or_assign(name, callback); +} + +void ComponentInterface::add_tf_broadcaster() { + if (this->tf_broadcaster_ == nullptr) { + RCLCPP_DEBUG(this->node_logging_->get_logger(), "Adding TF broadcaster."); + console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); + this->tf_broadcaster_ = std::make_shared( + this->node_parameters_, this->node_topics_, tf2_ros::DynamicBroadcasterQoS()); + } else { + RCLCPP_DEBUG(this->node_logging_->get_logger(), "TF broadcaster already exists."); + } +} + +void ComponentInterface::add_static_tf_broadcaster() { + if (this->static_tf_broadcaster_ == nullptr) { + RCLCPP_DEBUG(this->node_logging_->get_logger(), "Adding static TF broadcaster."); + console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); + tf2_ros::StaticBroadcasterQoS qos; + rclcpp::PublisherOptionsWithAllocator> options; + this->static_tf_broadcaster_ = + std::make_shared(this->node_parameters_, this->node_topics_, qos, options); + } else { + RCLCPP_DEBUG(this->node_logging_->get_logger(), "Static TF broadcaster already exists."); + } +} + +void ComponentInterface::add_tf_listener() { + if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { + RCLCPP_DEBUG(this->node_logging_->get_logger(), "Adding TF buffer and listener."); + console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); + this->tf_buffer_ = std::make_shared(this->node_clock_->get_clock()); + this->tf_listener_ = std::make_shared(*this->tf_buffer_); + } else { + RCLCPP_DEBUG(this->node_logging_->get_logger(), "TF buffer and listener already exist."); + } +} + +void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { + this->send_transforms(std::vector{transform}); +} + +void ComponentInterface::send_transforms(const std::vector& transforms) { + this->publish_transforms(transforms, this->tf_broadcaster_); +} + +void ComponentInterface::send_static_transform(const state_representation::CartesianPose& transform) { + this->send_static_transforms(std::vector{transform}); +} + +void ComponentInterface::send_static_transforms(const std::vector& transforms) { + this->publish_transforms(transforms, this->static_tf_broadcaster_, true); +} + +state_representation::CartesianPose ComponentInterface::lookup_transform( + const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point, + const tf2::Duration& duration +) { + auto transform = this->lookup_ros_transform(frame, reference_frame, time_point, duration); + state_representation::CartesianPose result(frame, reference_frame); + modulo_core::translators::read_message(result, transform); + return result; +} + +state_representation::CartesianPose ComponentInterface::lookup_transform( + const std::string& frame, const std::string& reference_frame, double validity_period, const tf2::Duration& duration +) { + auto transform = + this->lookup_ros_transform(frame, reference_frame, tf2::TimePoint(std::chrono::microseconds(0)), duration); + if (validity_period > 0.0 + && (this->node_clock_->get_clock()->now() - transform.header.stamp).seconds() > validity_period) { + throw exceptions::LookupTransformException("Failed to lookup transform: Latest transform is too old!"); + } + state_representation::CartesianPose result(frame, reference_frame); + modulo_core::translators::read_message(result, transform); + return result; +} + +geometry_msgs::msg::TransformStamped ComponentInterface::lookup_ros_transform( + const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point, + const tf2::Duration& duration +) { + if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { + throw exceptions::LookupTransformException("Failed to lookup transform: To TF buffer / listener configured."); + } + try { + return this->tf_buffer_->lookupTransform(reference_frame, frame, time_point, duration); + } catch (const tf2::TransformException& ex) { + throw exceptions::LookupTransformException(std::string("Failed to lookup transform: ").append(ex.what())); + } +} + +rclcpp::QoS ComponentInterface::get_qos() const { + return this->qos_; +} + +void ComponentInterface::set_qos(const rclcpp::QoS& qos) { + this->qos_ = qos; +} + +void ComponentInterface::raise_error() { + RCLCPP_DEBUG(this->node_logging_->get_logger(), "raise_error called: Setting predicate 'in_error_state' to true."); + this->set_predicate("in_error_state", true); +} + +void ComponentInterface::publish_predicate(const std::string& name) { + modulo_component_interfaces::msg::Predicate message; + message.component = this->node_base_->get_fully_qualified_name(); + message.predicate = name; + message.value = this->get_predicate(name); + this->predicate_publisher_->publish(message); +} + +void ComponentInterface::publish_predicates() { + for (const auto& predicate : this->predicates_) { + this->publish_predicate(predicate.first); + } +} + +void ComponentInterface::publish_outputs() { + for (const auto& [signal, publisher] : this->outputs_) { + try { + if (this->periodic_outputs_.at(signal)) { + publisher->publish(); + } + } catch (const modulo_core::exceptions::CoreException& ex) { + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, + "Failed to publish output '" << signal << "': " << ex.what()); + } + } +} + +void ComponentInterface::evaluate_periodic_callbacks() { + for (const auto& [name, callback] : this->periodic_callbacks_) { + try { + callback(); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, + "Failed to evaluate periodic function callback '" << name << "': " << ex.what()); + } + } +} }// namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index fc86c3fde..043664414 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -5,6 +5,7 @@ #include "modulo_core/EncodedState.hpp" #include "modulo_core/translators/message_writers.hpp" #include "modulo_utils/testutils/ServiceClient.hpp" +#include "modulo_components/exceptions/LookupTransformException.hpp" #include "test_modulo_components/component_public_interfaces.hpp" namespace modulo_components { From 4fcd1e5ffb8aa93a3801c600f90b3865107b2ae2 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Tue, 24 Oct 2023 17:33:33 +0200 Subject: [PATCH 4/5] fix: avoid conflict with get_parameter (#42) --- CHANGELOG.md | 1 + .../include/modulo_components/Component.hpp | 7 +++++++ .../include/modulo_components/LifecycleComponent.hpp | 7 +++++++ source/modulo_components/src/Component.cpp | 5 +++++ source/modulo_components/src/LifecycleComponent.cpp | 5 +++++ 5 files changed, 25 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index bacdef683..ef60d10d7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ Release Versions: ## Upcoming changes (in development) +- Avoid conflict with get_parameter (#42) - Revise ComponentInterface by moving implementations to source file (#39) - Update component classes to inherit from new ComponentInterface (#38) - Refactor ComponentInterface to use node interfaces (#33) diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 2b56251d0..f382fa5c8 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -37,6 +37,11 @@ class Component : public rclcpp::Node, public ComponentInterface { virtual ~Component() = default; protected: + /** + * @copydoc ComponentInterface::get_parameter + */ + [[nodiscard]] std::shared_ptr get_parameter(const std::string& name) const; + /** * @brief Start the execution thread. */ @@ -76,6 +81,7 @@ class Component : public rclcpp::Node, public ComponentInterface { void on_execute(); // TODO hide ROS methods + using ComponentInterface::get_parameter; using ComponentInterface::create_output; using ComponentInterface::inputs_; using ComponentInterface::outputs_; @@ -84,6 +90,7 @@ class Component : public rclcpp::Node, public ComponentInterface { using ComponentInterface::publish_predicates; using ComponentInterface::publish_outputs; using ComponentInterface::evaluate_periodic_callbacks; + using rclcpp::Node::get_parameter; std::thread execute_thread_; ///< The execution thread of the component bool started_; ///< Flag that indicates if execution has started or not diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 84d3539f4..13fde1616 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -45,6 +45,11 @@ class LifecycleComponent : public rclcpp_lifecycle::LifecycleNode, public Compon virtual ~LifecycleComponent() = default; protected: + /** + * @copydoc ComponentInterface::get_parameter + */ + [[nodiscard]] std::shared_ptr get_parameter(const std::string& name) const; + /** * @brief Steps to execute when configuring the component. * @details This method can be overridden by derived Component classes. @@ -256,6 +261,7 @@ class LifecycleComponent : public rclcpp_lifecycle::LifecycleNode, public Compon bool clear_signals(); // TODO hide ROS methods + using ComponentInterface::get_parameter; using ComponentInterface::create_output; using ComponentInterface::inputs_; using ComponentInterface::outputs_; @@ -264,6 +270,7 @@ class LifecycleComponent : public rclcpp_lifecycle::LifecycleNode, public Compon using ComponentInterface::publish_predicates; using ComponentInterface::publish_outputs; using ComponentInterface::evaluate_periodic_callbacks; + using rclcpp_lifecycle::LifecycleNode::get_parameter; }; template diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp index 61a08fb17..9eddba777 100644 --- a/source/modulo_components/src/Component.cpp +++ b/source/modulo_components/src/Component.cpp @@ -56,4 +56,9 @@ void Component::on_execute() { bool Component::on_execute_callback() { return true; } + +std::shared_ptr +Component::get_parameter(const std::string& name) const { + return ComponentInterface::get_parameter(name); +} }// namespace modulo_components diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index bbb119d94..0695b93f4 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -34,6 +34,11 @@ LifecycleComponent::LifecycleComponent(const rclcpp::NodeOptions& node_options, }); } +std::shared_ptr +LifecycleComponent::get_parameter(const std::string& name) const { + return ComponentInterface::get_parameter(name); +} + void LifecycleComponent::step() { try { if (this->get_predicate("is_active")) { From 13957a3b09bb769fce44553ab0e74f0df586d234 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Tue, 24 Oct 2023 17:33:52 +0200 Subject: [PATCH 5/5] fix: remove callback group for topics and services (#61) --- CHANGELOG.md | 1 + .../include/modulo_components/ComponentInterface.hpp | 1 - source/modulo_components/src/ComponentInterface.cpp | 7 +++---- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index ef60d10d7..40bbbeb59 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ Release Versions: ## Upcoming changes (in development) +- Remove callback group for topics and services (#61) - Avoid conflict with get_parameter (#42) - Revise ComponentInterface by moving implementations to source file (#39) - Update component classes to inherit from new ComponentInterface (#38) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 63231e9e5..2f9ff4f24 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -552,7 +552,6 @@ class ComponentInterface { std::shared_ptr parameter_cb_handle_; ///< ROS callback function handle for setting parameters - std::shared_ptr cb_group_; std::shared_ptr step_timer_; ///< Timer for the step function std::shared_ptr tf_buffer_; ///< TF buffer std::shared_ptr tf_listener_; ///< TF listener diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index 24cea1cbc..2f1370754 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -22,7 +22,6 @@ ComponentInterface::ComponentInterface( node_services_(interfaces->get_node_services_interface()), node_timers_(interfaces->get_node_timers_interface()), node_topics_(interfaces->get_node_topics_interface()) { - this->cb_group_ = node_base_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); // register the parameter change callback handler this->parameter_cb_handle_ = this->node_parameters_->add_on_set_parameters_callback( [this](const std::vector& parameters) -> rcl_interfaces::msg::SetParametersResult { @@ -38,7 +37,7 @@ ComponentInterface::ComponentInterface( this->step_timer_ = rclcpp::create_wall_timer( std::chrono::nanoseconds(static_cast(this->get_parameter_value("period") * 1e9)), - [this] { this->step(); }, this->cb_group_, this->node_base_.get(), this->node_timers_.get()); + [this] { this->step(); }, nullptr, this->node_base_.get(), this->node_timers_.get()); } void ComponentInterface::step() {} @@ -347,7 +346,7 @@ void ComponentInterface::add_service( response->success = false; response->message = ex.what(); } - }, this->qos_, this->cb_group_); + }, this->qos_, nullptr); this->empty_services_.insert_or_assign(parsed_service_name, service); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(), @@ -374,7 +373,7 @@ void ComponentInterface::add_service( response->success = false; response->message = ex.what(); } - }, this->qos_, this->cb_group_); + }, this->qos_, nullptr); this->string_services_.insert_or_assign(parsed_service_name, service); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM(this->node_logging_->get_logger(),