LifecycleComponent.hpp

#pragma once
 
#include <lifecycle_msgs/msg/state.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
 
#include "modulo_components/ComponentInterface.hpp"
 
namespace modulo_components {
 
class LifecycleComponent : public rclcpp_lifecycle::LifecycleNode, public ComponentInterface {
public:
  friend class LifecycleComponentPublicInterface;
 
  explicit LifecycleComponent(
      const rclcpp::NodeOptions& node_options, const std::string& fallback_name = "LifecycleComponent");
 
  virtual ~LifecycleComponent() = default;
 
protected:
  [[nodiscard]] std::shared_ptr<state_representation::ParameterInterface> get_parameter(const std::string& name) const;
 
  virtual bool on_configure_callback();
 
  virtual bool on_cleanup_callback();
 
  virtual bool on_activate_callback();
 
  virtual bool on_deactivate_callback();
 
  virtual bool on_shutdown_callback();
 
  virtual bool on_error_callback();
 
  template<typename DataT>
  void add_output(
      const std::string& signal_name, const std::shared_ptr<DataT>& data, const std::string& default_topic = "",
      bool fixed_topic = false, bool publish_on_step = true);
 
  rclcpp_lifecycle::State get_lifecycle_state() const;
 
private:
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_configure(const rclcpp_lifecycle::State& previous_state) override;
 
  bool handle_configure();
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_cleanup(const rclcpp_lifecycle::State& previous_state) override;
 
  bool handle_cleanup();
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_activate(const rclcpp_lifecycle::State& previous_state) override;
 
  bool handle_activate();
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
 
  bool handle_deactivate();
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_shutdown(const rclcpp_lifecycle::State& previous_state) override;
 
  bool handle_shutdown();
 
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_error(const rclcpp_lifecycle::State& previous_state) override;
 
  bool handle_error();
 
  void step() override;
 
  bool configure_outputs();
 
  bool activate_outputs();
 
  bool deactivate_outputs();
 
  bool clear_signals();
 
  // TODO hide ROS methods
  using ComponentInterface::create_output;
  using ComponentInterface::evaluate_periodic_callbacks;
  using ComponentInterface::get_parameter;
  using ComponentInterface::inputs_;
  using ComponentInterface::outputs_;
  using ComponentInterface::periodic_outputs_;
  using ComponentInterface::publish_outputs;
  using ComponentInterface::publish_predicates;
  using rclcpp_lifecycle::LifecycleNode::get_parameter;
 
  std::map<
      std::string,
      std::function<std::shared_ptr<modulo_core::communication::PublisherInterface>(const std::string& topic_name)>>
      custom_output_configuration_callables_;
};
 
template<typename DataT>
inline void LifecycleComponent::add_output(
    const std::string& signal_name, const std::shared_ptr<DataT>& data, const std::string& default_topic,
    bool fixed_topic, bool publish_on_step) {
  if (this->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED
      && this->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
    RCLCPP_WARN_STREAM_THROTTLE(
        this->get_logger(), *this->get_clock(), 1000,
        "Adding output in state " << this->get_lifecycle_state().label() << " is not allowed.");
    return;
  }
  try {
    using modulo_core::communication::PublisherHandler;
    using modulo_core::communication::PublisherType;
 
    auto parsed_signal_name = this->create_output(
        PublisherType::LIFECYCLE_PUBLISHER, signal_name, data, default_topic, fixed_topic, publish_on_step);
 
    auto message_pair = this->outputs_.at(parsed_signal_name)->get_message_pair();
    if (message_pair->get_type() == modulo_core::communication::MessageType::CUSTOM_MESSAGE) {
      if constexpr (modulo_core::concepts::CustomT<DataT>) {
        this->custom_output_configuration_callables_.insert_or_assign(
            parsed_signal_name, [this, message_pair](const std::string& topic_name) {
              auto publisher = this->create_publisher<DataT>(topic_name, this->get_qos());
              return std::make_shared<PublisherHandler<rclcpp_lifecycle::LifecyclePublisher<DataT>, DataT>>(
                         PublisherType::LIFECYCLE_PUBLISHER, publisher)
                  ->create_publisher_interface(message_pair);
            });
      }
    }
  } catch (const modulo_core::exceptions::AddSignalException& ex) {
    RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what());
  }
}
 
// TODO, if we raise error we need to manually call the lifecycle change state callback,
// call callback function that this service calls
}// namespace modulo_components