diff --git a/CHANGELOG.md b/CHANGELOG.md index df2ff7985..66ca4b8fe 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ Release Versions: ## Upcoming changes (in development) +- Refactor ComponentInterface to use node interfaces (#33) - Refactor component predicates with a single Predicate publisher (#26) - Add and install component descriptions (#31) - Apply AICA C++ style guide (#30) diff --git a/VERSION b/VERSION index 0d3ad67af..0b6e43134 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.2.10 +2.2.11 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 858f9d298..70b61119d 100644 --- a/doxygen/doxygen.conf +++ b/doxygen/doxygen.conf @@ -38,7 +38,7 @@ PROJECT_NAME = "Modulo" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 2.2.10 +PROJECT_NUMBER = 2.2.11 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/source/modulo_component_interfaces/package.xml b/source/modulo_component_interfaces/package.xml index e2fbbddad..6b4f820b6 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.2.10 + 2.2.11 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard GPLv3 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 151aef7fd..4eda624bc 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. */ @@ -354,8 +338,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 ); /** @@ -556,12 +540,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>> @@ -575,55 +556,39 @@ 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), 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("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 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) { @@ -632,8 +597,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 ) { @@ -644,8 +608,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; @@ -654,14 +618,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); } } @@ -670,13 +635,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) { @@ -684,46 +649,45 @@ 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() == "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) { @@ -752,40 +716,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; @@ -801,23 +762,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); @@ -829,24 +789,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; } @@ -854,56 +812,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); @@ -920,43 +872,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 ) { @@ -969,64 +917,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 ) { @@ -1035,19 +989,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( @@ -1061,15 +1016,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 ) { @@ -1081,22 +1035,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 ) { @@ -1108,95 +1062,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; @@ -1206,18 +1154,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 ) { @@ -1231,8 +1179,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 ) { @@ -1242,13 +1189,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); @@ -1256,24 +1203,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."); @@ -1284,42 +1228,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 { @@ -1329,12 +1270,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&) { @@ -1344,19 +1285,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/package.xml b/source/modulo_components/package.xml index 7dea8085b..8ee648847 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.2.10 + 2.2.11 Modulo base classes that wrap ROS2 Nodes as modular components for the AICA application framework Baptiste Busch Enrico Eberhard diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp new file mode 100644 index 000000000..3df38d3ad --- /dev/null +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -0,0 +1,33 @@ +#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("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_ = 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 741a2ff6a..730a900db 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -24,18 +24,20 @@ class ComponentInterfaceTest : public ::testing::Test { void SetUp() override { exec_ = std::make_shared(); + this->node_ = std::make_shared("ComponentInterfacePublicInterface", rclcpp::NodeOptions()); if (std::is_same::value) { - this->component_ = std::make_shared>( - rclcpp::NodeOptions(), modulo_core::communication::PublisherType::PUBLISHER); + this->pub_type_ = modulo_core::communication::PublisherType::PUBLISHER; + this->component_ = std::make_shared(this->node_); } else if (std::is_same::value) { - this->component_ = std::make_shared>( - rclcpp::NodeOptions(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER); + this->pub_type_ = modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER; + this->component_ = std::make_shared(this->node_); } } 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; @@ -126,7 +128,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); @@ -188,8 +191,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); @@ -208,8 +211,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"; @@ -223,7 +226,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")); @@ -237,7 +240,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; diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 96735b268..ef43cd76f 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.2.10 + 2.2.11 Modulo Core communication and translation utilities for interoperability with AICA Control Libraries Baptiste Busch Enrico Eberhard diff --git a/source/modulo_utils/package.xml b/source/modulo_utils/package.xml index ab5a4c96a..cecacdfae 100644 --- a/source/modulo_utils/package.xml +++ b/source/modulo_utils/package.xml @@ -2,7 +2,7 @@ modulo_utils - 2.2.10 + 2.2.11 Modulo utils package for shared test fixtures Dominic Reber GPLv3