diff --git a/CHANGELOG.md b/CHANGELOG.md index bf232bb6f..6044d9c31 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,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) - Refactor component predicates with a single Predicate publisher (#26) diff --git a/VERSION b/VERSION index 98c938ec3..f2b42fc71 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -2.2.12 +2.2.13 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 660f8a9a6..10d1faa15 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.12 +PROJECT_NUMBER = 2.2.13 # 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 512f8af58..d104f6258 100644 --- a/source/modulo_component_interfaces/package.xml +++ b/source/modulo_component_interfaces/package.xml @@ -2,7 +2,7 @@ modulo_component_interfaces - 2.2.12 + 2.2.13 Interface package for communicating with modulo components through the ROS framework Enrico Eberhard GPLv3 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 ee1260682..640ec7036 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" @@ -90,7 +83,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, @@ -106,7 +99,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); @@ -127,7 +120,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. @@ -226,18 +219,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 @@ -281,6 +262,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 @@ -324,43 +342,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 @@ -414,6 +395,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 @@ -435,24 +433,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 @@ -488,19 +468,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 @@ -514,6 +481,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 @@ -523,6 +503,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 @@ -573,8 +566,6 @@ class ComponentInterface { bool set_parameter_callback_called_ = false; ///< Flag to indicate if on_set_parameter_callback was called }; -inline void ComponentInterface::step() {} - template inline void ComponentInterface::add_parameter( const std::string& name, const T& value, const std::string& description, bool read_only @@ -596,58 +587,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 { @@ -665,235 +604,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() == "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, @@ -1001,141 +711,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, @@ -1163,137 +779,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 804f9f9a5..b82a7af79 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -265,13 +265,12 @@ 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; }; -inline void LifecycleComponent::on_step_callback() {} - template inline void LifecycleComponent::add_output( const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic, diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 1af680431..bb998b46a 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -2,7 +2,7 @@ modulo_components - 2.2.12 + 2.2.13 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 index 0808af043..257b396c0 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 { @@ -30,4 +38,505 @@ 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::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() == "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/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index d948b3aa7..11f40e590 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -33,6 +33,8 @@ LifecycleComponent::LifecycleComponent(const rclcpp::NodeOptions& node_options, }); } +void LifecycleComponent::on_step_callback() {} + void LifecycleComponent::step() { try { if (this->get_predicate("is_active")) { diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 730a900db..a44806336 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 { diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index bc3192d32..21539b437 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 2.2.12 + 2.2.13 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 bab009ca5..e438da73c 100644 --- a/source/modulo_utils/package.xml +++ b/source/modulo_utils/package.xml @@ -2,7 +2,7 @@ modulo_utils - 2.2.12 + 2.2.13 Modulo utils package for shared test fixtures Dominic Reber GPLv3