MessagePair.hpp

#pragma once
 
#include <rclcpp/clock.hpp>
 
#include "modulo_core/communication/MessagePairInterface.hpp"
#include "modulo_core/concepts.hpp"
#include "modulo_core/translators/message_readers.hpp"
#include "modulo_core/translators/message_writers.hpp"
 
namespace modulo_core::communication {
 
template<typename MsgT, typename DataT>
class MessagePair : public MessagePairInterface {
public:
  MessagePair(std::shared_ptr<DataT> data, std::shared_ptr<rclcpp::Clock> clock);
 
  MessagePair(std::shared_ptr<DataT> data, std::shared_ptr<rclcpp::Clock> clock)
    requires concepts::CustomT<MsgT> && concepts::CustomT<DataT>
      : MessagePairInterface(MessageType::CUSTOM_MESSAGE), data_(std::move(data)), clock_(std::move(clock)) {}
 
  [[nodiscard]] MsgT write_message() const;
 
  void read_message(const MsgT& message);
 
  [[nodiscard]] std::shared_ptr<DataT> get_data() const;
 
  void set_data(const std::shared_ptr<DataT>& data);
 
private:
  [[nodiscard]] MsgT write_translated_message() const;
 
  [[nodiscard]] MsgT write_encoded_message() const;
 
  [[nodiscard]] MsgT write_raw_message() const;
 
  void read_translated_message(const MsgT& message);
 
  void read_encoded_message(const MsgT& message);
 
  void read_raw_message(const MsgT& message);
 
  std::shared_ptr<DataT> data_;         
  std::shared_ptr<rclcpp::Clock> clock_;
};
 
template<typename MsgT, typename DataT>
inline MsgT MessagePair<MsgT, DataT>::write_message() const {
  if (this->data_ == nullptr) {
    throw exceptions::NullPointerException("The message pair data is not set, nothing to write");
  }
 
  MsgT message;
  if constexpr (concepts::CustomT<MsgT> && concepts::CustomT<DataT>) {
    message = write_raw_message();
  } else if constexpr (std::same_as<MsgT, EncodedState>) {
    message = write_encoded_message();
  } else {
    message = write_translated_message();
  }
  return message;
}
 
template<typename MsgT, typename DataT>
inline MsgT MessagePair<MsgT, DataT>::write_translated_message() const {
  auto message = MsgT();
  translators::write_message(message, *this->data_, clock_->now());
  return message;
}
 
template<>
inline EncodedState MessagePair<EncodedState, state_representation::State>::write_encoded_message() const {
  auto message = EncodedState();
  translators::write_message(message, this->data_, clock_->now());
  return message;
}
 
template<typename MsgT, typename DataT>
inline MsgT MessagePair<MsgT, DataT>::write_raw_message() const {
  return *this->data_;
}
 
template<typename MsgT, typename DataT>
inline void MessagePair<MsgT, DataT>::read_message(const MsgT& message) {
  if (this->data_ == nullptr) {
    throw exceptions::NullPointerException("The message pair data is not set, nothing to read");
  }
 
  if constexpr (concepts::CustomT<MsgT> && concepts::CustomT<DataT>) {
    read_raw_message(message);
  } else if constexpr (std::same_as<MsgT, EncodedState>) {
    read_encoded_message(message);
  } else {
    read_translated_message(message);
  }
}
 
template<typename MsgT, typename DataT>
inline void MessagePair<MsgT, DataT>::read_translated_message(const MsgT& message) {
  translators::read_message(*this->data_, message);
}
 
template<>
inline void MessagePair<EncodedState, state_representation::State>::read_encoded_message(const EncodedState& message) {
  translators::read_message(this->data_, message);
}
 
template<typename MsgT, typename DataT>
inline void MessagePair<MsgT, DataT>::read_raw_message(const MsgT& message) {
  *this->data_ = message;
}
 
template<typename MsgT, typename DataT>
inline std::shared_ptr<DataT> MessagePair<MsgT, DataT>::get_data() const {
  return this->data_;
}
 
template<typename MsgT, typename DataT>
inline void MessagePair<MsgT, DataT>::set_data(const std::shared_ptr<DataT>& data) {
  if (data == nullptr) {
    throw exceptions::NullPointerException("Provide a valid pointer");
  }
  this->data_ = data;
}
 
template<concepts::CoreDataT DataT>
inline std::shared_ptr<MessagePairInterface>
make_shared_message_pair(const std::shared_ptr<DataT>& data, const std::shared_ptr<rclcpp::Clock>& clock) {
  return std::make_shared<MessagePair<EncodedState, state_representation::State>>(
      std::dynamic_pointer_cast<state_representation::State>(data), clock);
}
 
template<concepts::CustomT DataT>
inline std::shared_ptr<MessagePairInterface>
make_shared_message_pair(const std::shared_ptr<DataT>& data, const std::shared_ptr<rclcpp::Clock>& clock) {
  return std::make_shared<MessagePair<DataT, DataT>>(data, clock);
}
 
template<typename DataT = bool>
inline std::shared_ptr<MessagePairInterface>
make_shared_message_pair(const std::shared_ptr<bool>& data, const std::shared_ptr<rclcpp::Clock>& clock) {
  return std::make_shared<MessagePair<std_msgs::msg::Bool, bool>>(data, clock);
}
 
template<typename DataT = double>
inline std::shared_ptr<MessagePairInterface>
make_shared_message_pair(const std::shared_ptr<double>& data, const std::shared_ptr<rclcpp::Clock>& clock) {
  return std::make_shared<MessagePair<std_msgs::msg::Float64, double>>(data, clock);
}
 
template<typename DataT = std::vector<double>>
inline std::shared_ptr<MessagePairInterface> make_shared_message_pair(
    const std::shared_ptr<std::vector<double>>& data, const std::shared_ptr<rclcpp::Clock>& clock) {
  return std::make_shared<MessagePair<std_msgs::msg::Float64MultiArray, std::vector<double>>>(data, clock);
}
 
template<typename DataT = int>
inline std::shared_ptr<MessagePairInterface>
make_shared_message_pair(const std::shared_ptr<int>& data, const std::shared_ptr<rclcpp::Clock>& clock) {
  return std::make_shared<MessagePair<std_msgs::msg::Int32, int>>(data, clock);
}
 
template<typename DataT = std::string>
inline std::shared_ptr<MessagePairInterface>
make_shared_message_pair(const std::shared_ptr<std::string>& data, const std::shared_ptr<rclcpp::Clock>& clock) {
  return std::make_shared<MessagePair<std_msgs::msg::String, std::string>>(data, clock);
}
}// namespace modulo_core::communication