From a4d63ebdc2e451640d7a9c64a07ce40ab789c6e6 Mon Sep 17 00:00:00 2001
From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com>
Date: Sun, 29 Sep 2024 13:02:40 +0200
Subject: [PATCH] fix: refine read_message conditions && perform the actual
 read

---
 .../modulo_core/communication/MessagePair.hpp   |  9 +++++----
 .../communication/SubscriptionHandler.hpp       |  6 +++---
 .../cpp/communication/test_communication.cpp    | 17 +++++++++++++++++
 3 files changed, 25 insertions(+), 7 deletions(-)

diff --git a/source/modulo_core/include/modulo_core/communication/MessagePair.hpp b/source/modulo_core/include/modulo_core/communication/MessagePair.hpp
index 50e7b0e6..99243572 100644
--- a/source/modulo_core/include/modulo_core/communication/MessagePair.hpp
+++ b/source/modulo_core/include/modulo_core/communication/MessagePair.hpp
@@ -122,16 +122,17 @@ inline void MessagePair<MsgT, DataT>::read_message(const MsgT& message) {
     throw exceptions::NullPointerException("The message pair data is not set, nothing to read");
   }
 
-  if constexpr (TranslatedDataT<MsgT> && !CustomDataT<DataT>) {
-    read_translated_message(message);
-  } else if constexpr (std::same_as<MsgT, EncodedState>) {
+  if constexpr (std::same_as<MsgT, EncodedState>) {
     read_encoded_message(message);
-  } else if constexpr (CustomDataT<MsgT> && CustomDataT<DataT>) {
+  } else if constexpr (std::same_as<MsgT, DataT> || (CustomDataT<MsgT> && CustomDataT<DataT>) ) {
     read_raw_message(message);
+  } else if constexpr (TranslatedDataT<MsgT> || CoreDataT<MsgT>) {
+    read_translated_message(message);
   } else {
     static_assert(false, "The message types for the message pair are not supported.");
   }
 }
+
 template<typename MsgT, typename DataT>
 inline void MessagePair<MsgT, DataT>::read_translated_message(const MsgT& message) {
   translators::read_message(*this->data_, message);
diff --git a/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp b/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp
index 6ae0180c..00077bf9 100644
--- a/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp
+++ b/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp
@@ -118,10 +118,10 @@ 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_();
+      this->get_message_pair()->template read<MsgT, MsgT>(*message);
+      this->user_callback_();
     } catch (...) {
-      //   this->handle_callback_exceptions();
+      this->handle_callback_exceptions();
     }
   };
 }
diff --git a/source/modulo_core/test/cpp/communication/test_communication.cpp b/source/modulo_core/test/cpp/communication/test_communication.cpp
index 850ded18..df3eb243 100644
--- a/source/modulo_core/test/cpp/communication/test_communication.cpp
+++ b/source/modulo_core/test/cpp/communication/test_communication.cpp
@@ -2,6 +2,9 @@
 
 #include "test_modulo_core/communication_nodes.hpp"
 
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+
 using namespace modulo_core::communication;
 
 class CommunicationTest : public ::testing::Test {
@@ -60,3 +63,17 @@ TEST_F(CommunicationTest, BasicTypes) {
   this->communicate<std_msgs::msg::Int32, int>(1, 2);
   this->communicate<std_msgs::msg::String, std::string>("this", "that");
 }
+
+TEST_F(CommunicationTest, CustomTypes) {
+  sensor_msgs::msg::Image initial_image;
+  initial_image.height = 480;
+  sensor_msgs::msg::Image new_image;
+  new_image.height = 320;
+  this->communicate<sensor_msgs::msg::Image, sensor_msgs::msg::Image>(initial_image, new_image);
+
+  //   sensor_msgs::msg::Imu initial_imu;
+  //   initial_imu.linear_acceleration.x = 1.0;
+  //   sensor_msgs::msg::Imu new_imu;
+  //   new_imu.linear_acceleration.x = 0.5;
+  //   this->communicate<sensor_msgs::msg::Imu, sensor_msgs::msg::Imu>(initial_imu, new_imu);
+}