SubscriptionHandler.hpp

#pragma once
 
#include "modulo_core/communication/SubscriptionInterface.hpp"
 
#include "modulo_core/concepts.hpp"
 
namespace modulo_core::communication {
 
template<typename MsgT>
class SubscriptionHandler : public SubscriptionInterface {
public:
  explicit SubscriptionHandler(
      std::shared_ptr<MessagePairInterface> message_pair = nullptr,
      const rclcpp::Logger& logger = rclcpp::get_logger("SubscriptionHandler"));
 
  ~SubscriptionHandler() override;
 
  [[nodiscard]] std::shared_ptr<rclcpp::Subscription<MsgT>> get_subscription() const;
 
  void set_subscription(const std::shared_ptr<rclcpp::Subscription<MsgT>>& subscription);
 
  void set_user_callback(const std::function<void()>& user_callback);
 
  std::function<void(const std::shared_ptr<MsgT>)> get_callback();
 
  std::function<void(const std::shared_ptr<MsgT>)> get_callback(const std::function<void()>& user_callback);
 
  std::shared_ptr<SubscriptionInterface>
  create_subscription_interface(const std::shared_ptr<rclcpp::Subscription<MsgT>>& subscription);
 
private:
  void handle_callback_exceptions();
 
  std::function<void(const std::shared_ptr<MsgT>)> get_translated_callback();
 
  std::function<void(const std::shared_ptr<MsgT>)> get_raw_callback();
 
  std::shared_ptr<rclcpp::Subscription<MsgT>> subscription_;
  rclcpp::Logger logger_;                                   
  std::shared_ptr<rclcpp::Clock> clock_;                    
  std::function<void()> user_callback_ = [] {
  };
};
 
template<typename MsgT>
SubscriptionHandler<MsgT>::SubscriptionHandler(
    std::shared_ptr<MessagePairInterface> message_pair, const rclcpp::Logger& logger)
    : SubscriptionInterface(std::move(message_pair)), logger_(logger), clock_(std::make_shared<rclcpp::Clock>()) {}
 
template<typename MsgT>
SubscriptionHandler<MsgT>::~SubscriptionHandler() {
  this->subscription_.reset();
}
 
template<typename MsgT>
std::shared_ptr<rclcpp::Subscription<MsgT>> SubscriptionHandler<MsgT>::get_subscription() const {
  return this->subscription_;
}
 
template<typename MsgT>
void SubscriptionHandler<MsgT>::set_subscription(const std::shared_ptr<rclcpp::Subscription<MsgT>>& subscription) {
  if (subscription == nullptr) {
    throw exceptions::NullPointerException("Provide a valid pointer");
  }
  this->subscription_ = subscription;
}
 
template<typename MsgT>
inline std::function<void(const std::shared_ptr<MsgT>)> SubscriptionHandler<MsgT>::get_callback() {
  if (this->message_pair_ == nullptr) {
    throw exceptions::NullPointerException("Message pair is not set, can't get subscription callback");
  }
  if constexpr (concepts::TranslatedMsgT<MsgT>) {
    if (this->message_pair_->get_type() == MessageType::CUSTOM_MESSAGE) {
      if constexpr (concepts::CustomT<MsgT>) {
        return get_raw_callback();
      }
      __builtin_unreachable();
    } else {
      return get_translated_callback();
    }
  } else {
    return get_raw_callback();
  }
}
 
template<typename MsgT>
std::function<void(const std::shared_ptr<MsgT>)> SubscriptionHandler<MsgT>::get_raw_callback() {
  return [this](const std::shared_ptr<MsgT> message) {
    try {
      this->get_message_pair()->template read<MsgT, MsgT>(*message);
      this->user_callback_();
    } catch (...) {
      this->handle_callback_exceptions();
    }
  };
}
 
template<typename MsgT>
void SubscriptionHandler<MsgT>::set_user_callback(const std::function<void()>& user_callback) {
  this->user_callback_ = user_callback;
}
 
template<typename MsgT>
std::function<void(const std::shared_ptr<MsgT>)>
SubscriptionHandler<MsgT>::get_callback(const std::function<void()>& user_callback) {
  this->set_user_callback(user_callback);
  return this->get_callback();
}
 
template<typename MsgT>
std::shared_ptr<SubscriptionInterface> SubscriptionHandler<MsgT>::create_subscription_interface(
    const std::shared_ptr<rclcpp::Subscription<MsgT>>& subscription) {
  this->set_subscription(subscription);
  return std::shared_ptr<SubscriptionInterface>(this->shared_from_this());
}
 
template<typename MsgT>
void SubscriptionHandler<MsgT>::handle_callback_exceptions() {
  try {
    // re-throw the original exception
    throw;
  } catch (const exceptions::CoreException& ex) {
    RCLCPP_WARN_STREAM_THROTTLE(
        this->logger_, *this->clock_, 1000, "Exception in subscription callback: " << ex.what());
  } catch (const std::exception& ex) {
    RCLCPP_WARN_STREAM_THROTTLE(
        this->logger_, *this->clock_, 1000, "Unhandled exception in subscription user callback: " << ex.what());
  }
}
 
}// namespace modulo_core::communication