From e53f6cb585b7e70b1398dc69d34b1b4ffc632fd0 Mon Sep 17 00:00:00 2001
From: Vaios Papaspyros <8146703+bpapaspyros@users.noreply.github.com>
Date: Thu, 3 Oct 2024 12:31:55 +0200
Subject: [PATCH] fix: bad_any_cast issue for custom publisher configuration
 callable

---
 .../BaseControllerInterface.hpp               | 10 ++++++----
 .../src/BaseControllerInterface.cpp           |  4 +++-
 .../test/test_controller_interface.cpp        | 20 +++++++++++++++++++
 .../include/modulo_core/concepts.hpp          |  5 +----
 4 files changed, 30 insertions(+), 9 deletions(-)

diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp
index 1e3c5bdd..a43f2526 100644
--- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp
+++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp
@@ -584,12 +584,14 @@ inline void BaseControllerInterface::add_output(const std::string& name, const s
       outputs_.insert_or_assign(name, PublisherT());
       custom_output_configuration_callables_.insert_or_assign(
           name, [this](CustomPublishers& pub, const std::string& name) {
-            auto publishers = std::any_cast<PublisherT>(pub);
-            publishers.first = get_node()->create_publisher<T>(name, qos_);
-            publishers.second = std::make_shared<realtime_tools::RealtimePublisher<T>>(publishers.first);
+            std::shared_ptr<rclcpp::Publisher<T>> publisher =
+                std::any_cast<std::shared_ptr<rclcpp::Publisher<T>>>(pub.first);
+            publisher = get_node()->create_publisher<T>(name, qos_);
+            realtime_tools::RealtimePublisherSharedPtr<T> realtime_publisher =
+                std::any_cast<realtime_tools::RealtimePublisherSharedPtr<T>>(pub.second);
+            realtime_publisher = std::make_shared<realtime_tools::RealtimePublisher<T>>(publisher);
           });
     }
-
   } else {
     std::shared_ptr<state_representation::State> state_ptr = std::make_shared<T>();
     create_output(EncodedStatePublishers(state_ptr, {}, {}), name, topic_name);
diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp
index ea9620d4..65b22327 100644
--- a/source/modulo_controllers/src/BaseControllerInterface.cpp
+++ b/source/modulo_controllers/src/BaseControllerInterface.cpp
@@ -408,8 +408,10 @@ void BaseControllerInterface::add_outputs() {
                 custom_output_configuration_callables_.at(name)(pub, name);
               }},
           publishers);
+    } catch (const std::bad_any_cast& ex) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Failed to add custom output '%s': %s", name.c_str(), ex.what());
     } catch (const std::exception& ex) {
-      RCLCPP_ERROR(get_node()->get_logger(), "Failed to add input '%s': %s", name.c_str(), ex.what());
+      RCLCPP_ERROR(get_node()->get_logger(), "Failed to add output '%s': %s", name.c_str(), ex.what());
     }
   }
 }
diff --git a/source/modulo_controllers/test/test_controller_interface.cpp b/source/modulo_controllers/test/test_controller_interface.cpp
index d029a0d3..7917aecb 100644
--- a/source/modulo_controllers/test/test_controller_interface.cpp
+++ b/source/modulo_controllers/test/test_controller_interface.cpp
@@ -191,8 +191,28 @@ TYPED_TEST_P(ControllerInterfaceTest, OutputTest) {
 TYPED_TEST_P(ControllerInterfaceTest, CustomOutputTest) {
   auto interface = std::make_unique<FriendControllerInterface>();
   interface->init("controller_interface", "", 0, "", interface->define_custom_node_options());
+  interface->get_node()->set_parameter({"hardware_name", "test"});
+  interface->get_node()->set_parameter({"input_validity_period", 0.1});
 
   interface->add_output<sensor_msgs::msg::Image>("output", "/output");
+  auto node_state = interface->get_node()->configure();
+  ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
+
+  //   rclcpp::Node test_node("test_node");
+  //   auto publisher = test_node.create_publisher<MsgT>("/input", rclcpp::SystemDefaultsQoS());
+
+  //   for (auto [message_data, write_func, read_func, validation_func] : this->test_cases_) {
+  //     message_data = write_func(message_data);
+  //     auto message = std::get<1>(message_data);
+  //     publisher->publish(message);
+  //     rclcpp::spin_some(this->interface_->get_node()->get_node_base_interface());
+  //     auto input = this->interface_->template read_input<DataT>("input");
+  //     ASSERT_TRUE(input);
+  //     EXPECT_TRUE(validation_func(message_data, std::make_tuple(*input, message)));
+  //     std::this_thread::sleep_for(100ms);
+  //     ASSERT_FALSE(this->interface_->template read_input<DataT>("input"));
+  //   }
+
   EXPECT_TRUE(true);
 }
 
diff --git a/source/modulo_core/include/modulo_core/concepts.hpp b/source/modulo_core/include/modulo_core/concepts.hpp
index 410c0014..a721241f 100644
--- a/source/modulo_core/include/modulo_core/concepts.hpp
+++ b/source/modulo_core/include/modulo_core/concepts.hpp
@@ -28,9 +28,6 @@ concept TranslatedMsgT = std::same_as<T, std_msgs::msg::Bool> || std::same_as<T,
     || std::same_as<T, std_msgs::msg::String> || std::same_as<T, modulo_core::EncodedState>;
 
 template<typename T>
-concept EncodedT = std::same_as<T, modulo_core::EncodedState>;
-
-template<typename T>
-concept CustomT = !CoreDataT<T> && !EncodedT<T>;
+concept CustomT = !CoreDataT<T> && !std::same_as<T, modulo_core::EncodedState>;
 
 }// namespace modulo_core::concepts