PublisherHandler.hpp

#pragma once
 
#include <rclcpp/logging.hpp>
 
#include "modulo_core/communication/PublisherInterface.hpp"
 
#include <rclcpp_lifecycle/lifecycle_publisher.hpp>
 
namespace modulo_core::communication {
 
template<typename PubT, typename MsgT>
class PublisherHandler : public PublisherInterface {
public:
  PublisherHandler(PublisherType type, std::shared_ptr<PubT> publisher);
 
  ~PublisherHandler() override;
 
  virtual void activate() override;
 
  virtual void deactivate() override;
 
  void publish() override;
 
  void publish(const MsgT& message) const;
 
  std::shared_ptr<PublisherInterface>
  create_publisher_interface(const std::shared_ptr<MessagePairInterface>& message_pair);
 
private:
  std::shared_ptr<PubT> publisher_;
 
  using PublisherInterface::message_pair_;
};
 
template<typename PubT, typename MsgT>
PublisherHandler<PubT, MsgT>::PublisherHandler(PublisherType type, std::shared_ptr<PubT> publisher)
    : PublisherInterface(type), publisher_(std::move(publisher)) {}
 
template<typename PubT, typename MsgT>
PublisherHandler<PubT, MsgT>::~PublisherHandler() {
  this->publisher_.reset();
}
 
template<typename PubT, typename MsgT>
inline void PublisherHandler<PubT, MsgT>::activate() {
  if constexpr (std::derived_from<PubT, rclcpp_lifecycle::LifecyclePublisher<MsgT>>) {
    if (this->publisher_ == nullptr) {
      throw exceptions::NullPointerException("Publisher not set");
    }
    try {
      this->publisher_->on_activate();
    } catch (const std::exception& ex) {
      throw exceptions::CoreException(ex.what());
    }
  }
}
 
template<typename PubT, typename MsgT>
inline void PublisherHandler<PubT, MsgT>::deactivate() {
  if constexpr (std::derived_from<PubT, rclcpp_lifecycle::LifecyclePublisher<MsgT>>) {
    if (this->publisher_ == nullptr) {
      throw exceptions::NullPointerException("Publisher not set");
    }
    try {
      this->publisher_->on_deactivate();
    } catch (const std::exception& ex) {
      throw exceptions::CoreException(ex.what());
    }
  }
}
 
template<typename PubT, typename MsgT>
inline void PublisherHandler<PubT, MsgT>::publish() {
  try {
    if constexpr (concepts::CustomT<MsgT> && !concepts::TranslatedMsgT<MsgT>) {
      if (this->message_pair_ == nullptr) {
        throw exceptions::NullPointerException("Message pair is not set, nothing to publish");
      }
      publish(this->message_pair_->write<MsgT, MsgT>());
    } else {
      PublisherInterface::publish();
    }
  } catch (const exceptions::CoreException& ex) {
    throw;
  }
}
 
template<typename PubT, typename MsgT>
void PublisherHandler<PubT, MsgT>::publish(const MsgT& message) const {
  if (this->publisher_ == nullptr) {
    throw exceptions::NullPointerException("Publisher not set");
  }
  try {
    this->publisher_->publish(message);
  } catch (const std::exception& ex) {
    throw exceptions::CoreException(ex.what());
  }
}
 
template<typename PubT, typename MsgT>
inline std::shared_ptr<PublisherInterface>
PublisherHandler<PubT, MsgT>::create_publisher_interface(const std::shared_ptr<MessagePairInterface>& message_pair) {
  std::shared_ptr<PublisherInterface> publisher_interface;
  try {
    publisher_interface = std::shared_ptr<PublisherInterface>(this->shared_from_this());
  } catch (const std::exception& ex) {
    throw exceptions::CoreException(ex.what());
  }
  publisher_interface->set_message_pair(message_pair);
  return publisher_interface;
}
}// namespace modulo_core::communication