ComponentInterface.hpp

#pragma once
 
#include <rclcpp/node_interfaces/node_interfaces.hpp>
 
#include <tf2_ros/buffer.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
 
#include <state_representation/parameters/ParameterMap.hpp>
 
#include <modulo_core/Predicate.hpp>
#include <modulo_core/communication/PublisherHandler.hpp>
#include <modulo_core/communication/PublisherType.hpp>
#include <modulo_core/communication/SubscriptionHandler.hpp>
#include <modulo_core/concepts.hpp>
#include <modulo_core/exceptions.hpp>
#include <modulo_core/translators/parameter_translators.hpp>
 
#include <modulo_interfaces/msg/predicate_collection.hpp>
#include <modulo_interfaces/srv/empty_trigger.hpp>
#include <modulo_interfaces/srv/string_trigger.hpp>
 
#include <modulo_utils/parsing.hpp>
 
namespace modulo_components {
 
struct ComponentServiceResponse {
  bool success;
  std::string message;
};
 
class ComponentInterfacePublicInterface;
 
class ComponentInterface {
public:
  friend class ComponentInterfacePublicInterface;
 
  virtual ~ComponentInterface();
 
protected:
  explicit ComponentInterface(
      const std::shared_ptr<rclcpp::node_interfaces::NodeInterfaces<ALL_RCLCPP_NODE_INTERFACES>>& interfaces);
 
  double get_rate() const;
 
  template<typename T>
  T get_period() const;
 
  virtual void step();
 
  virtual void on_step_callback();
 
  void add_parameter(
      const std::shared_ptr<state_representation::ParameterInterface>& parameter, const std::string& description,
      bool read_only = false);
 
  template<typename T>
  void add_parameter(const std::string& name, const T& value, const std::string& description, bool read_only = false);
 
  [[nodiscard]] std::shared_ptr<state_representation::ParameterInterface> get_parameter(const std::string& name) const;
 
  template<typename T>
  [[nodiscard]] T get_parameter_value(const std::string& name) const;
 
  template<typename T>
  void set_parameter_value(const std::string& name, const T& value);
 
  virtual bool
  on_validate_parameter_callback(const std::shared_ptr<state_representation::ParameterInterface>& parameter);
 
  void add_predicate(const std::string& predicate_name, bool predicate_value);
 
  void add_predicate(const std::string& predicate_name, const std::function<bool(void)>& predicate_function);
 
  [[nodiscard]] bool get_predicate(const std::string& predicate_name) const;
 
  void set_predicate(const std::string& predicate_name, bool predicate_value);
 
  void set_predicate(const std::string& predicate_name, const std::function<bool(void)>& predicate_function);
 
  void add_trigger(const std::string& trigger_name);
 
  void trigger(const std::string& trigger_name);
 
  void declare_input(const std::string& signal_name, const std::string& default_topic = "", bool fixed_topic = false);
 
  void declare_output(const std::string& signal_name, const std::string& default_topic = "", bool fixed_topic = false);
 
  template<typename DataT>
  void add_input(
      const std::string& signal_name, const std::shared_ptr<DataT>& data, const std::string& default_topic = "",
      bool fixed_topic = false);
 
  template<typename DataT>
  void add_input(
      const std::string& signal_name, const std::shared_ptr<DataT>& data, const std::function<void()>& callback,
      const std::string& default_topic = "", bool fixed_topic = false);
 
  template<typename MsgT>
  void add_input(
      const std::string& signal_name, const std::function<void(const std::shared_ptr<MsgT>)>& callback,
      const std::string& default_topic = "", bool fixed_topic = false);
 
  template<typename DataT>
  std::string create_output(
      modulo_core::communication::PublisherType publisher_type, const std::string& signal_name,
      const std::shared_ptr<DataT>& data, const std::string& default_topic, bool fixed_topic, bool publish_on_step);
 
  void publish_output(const std::string& signal_name);
 
  void remove_input(const std::string& signal_name);
 
  void remove_output(const std::string& signal_name);
 
  void add_service(const std::string& service_name, const std::function<ComponentServiceResponse(void)>& callback);
 
  void add_service(
      const std::string& service_name,
      const std::function<ComponentServiceResponse(const std::string& string)>& callback);
 
  void add_periodic_callback(const std::string& name, const std::function<void(void)>& callback);
 
  void add_tf_broadcaster();
 
  void add_static_tf_broadcaster();
 
  void add_tf_listener();
 
  void send_transform(const state_representation::CartesianPose& transform);
 
  void send_transforms(const std::vector<state_representation::CartesianPose>& transforms);
 
  void send_static_transform(const state_representation::CartesianPose& transform);
 
  void send_static_transforms(const std::vector<state_representation::CartesianPose>& transforms);
 
  [[nodiscard]] state_representation::CartesianPose lookup_transform(
      const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
      const tf2::Duration& duration);
 
  [[nodiscard]] state_representation::CartesianPose lookup_transform(
      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)));
 
  [[nodiscard]] rclcpp::QoS get_qos() const;
 
  void set_qos(const rclcpp::QoS& qos);
 
  virtual void raise_error();
 
  void publish_predicates();
 
  void publish_outputs();
 
  void evaluate_periodic_callbacks();
 
  void finalize_interfaces();
 
  std::map<std::string, std::shared_ptr<modulo_core::communication::SubscriptionInterface>> inputs_;
  std::map<std::string, std::shared_ptr<modulo_core::communication::PublisherInterface>> outputs_;  
  std::map<std::string, bool> periodic_outputs_;
 
private:
  void pre_set_parameters_callback(std::vector<rclcpp::Parameter>& parameters);
 
  rcl_interfaces::msg::SetParametersResult on_set_parameters_callback(const std::vector<rclcpp::Parameter>& parameters);
 
  bool validate_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter);
 
  modulo_interfaces::msg::Predicate get_predicate_message(const std::string& name, bool value) const;
 
  void declare_signal(
      const std::string& signal_name, const std::string& type, const std::string& default_topic, bool fixed_topic);
 
  template<typename T>
  bool remove_signal(
      const std::string& signal_name, std::map<std::string, std::shared_ptr<T>>& signal_map, bool skip_check = false);
 
  std::string validate_service_name(const std::string& service_name, const std::string& type) const;
 
  void publish_predicate(const std::string& predicate_name, bool value) const;
 
  template<typename T>
  void publish_transforms(
      const std::vector<state_representation::CartesianPose>& transforms, const std::shared_ptr<T>& tf_broadcaster,
      bool is_static = false);
 
  [[nodiscard]] geometry_msgs::msg::TransformStamped lookup_ros_transform(
      const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
      const tf2::Duration& duration);
 
  double rate_;          
  double period_;        
  std::mutex step_mutex_;
 
  std::map<std::string, modulo_core::Predicate> predicates_;
  std::shared_ptr<rclcpp::Publisher<modulo_interfaces::msg::PredicateCollection>>
      predicate_publisher_;
  modulo_interfaces::msg::PredicateCollection predicate_message_;
  std::vector<std::string> triggers_;
 
  std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::EmptyTrigger>>>
      empty_services_;
  std::map<std::string, std::shared_ptr<rclcpp::Service<modulo_interfaces::srv::StringTrigger>>>
      string_services_;
 
  std::map<std::string, std::function<void(void)>> periodic_callbacks_;
 
  state_representation::ParameterMap parameter_map_;
  std::unordered_map<std::string, bool> read_only_parameters_;
  std::shared_ptr<rclcpp::node_interfaces::PreSetParametersCallbackHandle>
      pre_set_parameter_cb_handle_;
  std::shared_ptr<rclcpp::node_interfaces::OnSetParametersCallbackHandle>
      on_set_parameter_cb_handle_;
  rcl_interfaces::msg::SetParametersResult set_parameters_result_;
  bool pre_set_parameter_callback_called_ = false;
 
  std::shared_ptr<rclcpp::TimerBase> step_timer_;                             
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;                                
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;                   
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;             
  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;
 
  std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_;
  std::shared_ptr<rclcpp::node_interfaces::NodeClockInterface> node_clock_;
  std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> node_logging_;
  std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> node_parameters_;
  std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services_;
  std::shared_ptr<rclcpp::node_interfaces::NodeTimersInterface> node_timers_;
  std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> node_topics_;
  rclcpp::QoS qos_ = rclcpp::QoS(10);
};
 
template<typename T>
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->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<typename T>
inline T ComponentInterface::get_parameter_value(const std::string& name) const {
  try {
    return this->parameter_map_.template get_parameter_value<T>(name);
  } catch (const state_representation::exceptions::InvalidParameterException& ex) {
    throw modulo_core::exceptions::ParameterException(
        "Failed to get parameter value of parameter '" + name + "': " + ex.what());
  }
}
 
template<typename T>
inline void ComponentInterface::set_parameter_value(const std::string& name, const T& value) {
  try {
    std::vector<rclcpp::Parameter> parameters{
        modulo_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))};
    this->pre_set_parameters_callback(parameters);
    this->pre_set_parameter_callback_called_ = true;
    auto result = this->node_parameters_->set_parameters(parameters).at(0);
    if (!result.successful) {
      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->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
        "Failed to set parameter value of parameter '" << name << "': " << ex.what());
  }
}
 
template<typename DataT>
inline void ComponentInterface::add_input(
    const std::string& signal_name, const std::shared_ptr<DataT>& data, const std::string& default_topic,
    bool fixed_topic) {
  this->add_input(signal_name, data, [] {}, default_topic, fixed_topic);
}
 
template<typename DataT>
inline void ComponentInterface::add_input(
    const std::string& signal_name, const std::shared_ptr<DataT>& data, const std::function<void()>& user_callback,
    const std::string& default_topic, bool fixed_topic) {
  using namespace modulo_core::communication;
  try {
    if (data == nullptr) {
      throw modulo_core::exceptions::NullPointerException("Invalid data pointer for input '" + signal_name + "'.");
    }
    this->declare_input(signal_name, default_topic, fixed_topic);
    std::string parsed_signal_name = modulo_utils::parsing::parse_topic_name(signal_name);
    auto topic_name = this->get_parameter_value<std::string>(parsed_signal_name + "_topic");
    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->node_clock_->get_clock());
    std::shared_ptr<SubscriptionInterface> subscription_interface;
    switch (message_pair->get_type()) {
      case MessageType::BOOL: {
        auto subscription_handler =
            std::make_shared<SubscriptionHandler<std_msgs::msg::Bool>>(message_pair, this->node_logging_->get_logger());
        auto subscription = rclcpp::create_subscription<std_msgs::msg::Bool>(
            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<SubscriptionHandler<std_msgs::msg::Float64>>(
            message_pair, this->node_logging_->get_logger());
        auto subscription = rclcpp::create_subscription<std_msgs::msg::Float64>(
            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<SubscriptionHandler<std_msgs::msg::Float64MultiArray>>(
            message_pair, this->node_logging_->get_logger());
        auto subscription = rclcpp::create_subscription<std_msgs::msg::Float64MultiArray>(
            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<SubscriptionHandler<std_msgs::msg::Int32>>(
            message_pair, this->node_logging_->get_logger());
        auto subscription = rclcpp::create_subscription<std_msgs::msg::Int32>(
            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<SubscriptionHandler<std_msgs::msg::String>>(
            message_pair, this->node_logging_->get_logger());
        auto subscription = rclcpp::create_subscription<std_msgs::msg::String>(
            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<SubscriptionHandler<modulo_core::EncodedState>>(
            message_pair, this->node_logging_->get_logger());
        auto subscription = rclcpp::create_subscription<modulo_core::EncodedState>(
            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::CUSTOM_MESSAGE: {
        if constexpr (modulo_core::concepts::CustomT<DataT>) {
          auto subscription_handler =
              std::make_shared<SubscriptionHandler<DataT>>(message_pair, this->node_logging_->get_logger());
          auto subscription = rclcpp::create_subscription<DataT>(
              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->node_logging_->get_logger(), "Failed to add input '" << signal_name << "': " << ex.what());
  }
}
 
template<typename MsgT>
inline void ComponentInterface::add_input(
    const std::string& signal_name, const std::function<void(const std::shared_ptr<MsgT>)>& callback,
    const std::string& default_topic, bool fixed_topic) {
  using namespace modulo_core::communication;
  try {
    std::string parsed_signal_name = modulo_utils::parsing::parse_topic_name(signal_name);
    this->declare_input(parsed_signal_name, default_topic, fixed_topic);
    auto topic_name = this->get_parameter_value<std::string>(parsed_signal_name + "_topic");
    RCLCPP_DEBUG_STREAM(
        this->node_logging_->get_logger(),
        "Adding input '" << parsed_signal_name << "' with topic name '" << topic_name << "'.");
    auto subscription = rclcpp::create_subscription<MsgT>(
        this->node_parameters_, this->node_topics_, topic_name, this->qos_,
        [this, signal_name, callback](const std::shared_ptr<MsgT> message) {
          try {
            callback(message);
          } catch (const std::exception& ex) {
            RCLCPP_WARN_STREAM_THROTTLE(
                this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
                "Unhandled exception in callback of input '" << signal_name << "': " << ex.what());
          }
        });
    auto subscription_interface =
        std::make_shared<SubscriptionHandler<MsgT>>()->create_subscription_interface(subscription);
    this->inputs_.insert_or_assign(parsed_signal_name, subscription_interface);
  } catch (const std::exception& ex) {
    RCLCPP_ERROR_STREAM(
        this->node_logging_->get_logger(), "Failed to add input '" << signal_name << "': " << ex.what());
  }
}
 
template<typename DataT>
inline std::string ComponentInterface::create_output(
    modulo_core::communication::PublisherType publisher_type, const std::string& signal_name,
    const std::shared_ptr<DataT>& data, const std::string& default_topic, bool fixed_topic, bool publish_on_step) {
  using namespace modulo_core::communication;
  try {
    if (data == nullptr) {
      throw modulo_core::exceptions::NullPointerException("Invalid data pointer for output '" + signal_name + "'.");
    }
    this->declare_output(signal_name, default_topic, fixed_topic);
    auto parsed_signal_name = modulo_utils::parsing::parse_topic_name(signal_name);
    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<PublisherInterface>(publisher_type, message_pair));
    this->periodic_outputs_.insert_or_assign(parsed_signal_name, publish_on_step);
    return parsed_signal_name;
  } catch (const modulo_core::exceptions::AddSignalException&) {
    throw;
  } catch (const std::exception& ex) {
    throw modulo_core::exceptions::AddSignalException(ex.what());
  }
}
 
template<typename T>
inline bool ComponentInterface::remove_signal(
    const std::string& signal_name, std::map<std::string, std::shared_ptr<T>>& 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);
  }
}
 
template<typename T>
inline void ComponentInterface::publish_transforms(
    const std::vector<state_representation::CartesianPose>& transforms, const std::shared_ptr<T>& tf_broadcaster,
    bool is_static) {
  std::string modifier = is_static ? "static " : "";
  if (tf_broadcaster == nullptr) {
    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;
  }
  try {
    std::vector<geometry_msgs::msg::TransformStamped> transform_messages;
    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->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->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000,
        "Failed to send " << modifier << "transform: " << ex.what());
  }
}
}// namespace modulo_components