feat: expose generic inputs and outputs to components

source/modulo_components/include/modulo_components/Component.hpp

@@ -5,6 +5,7 @@
 #include <rclcpp/node.hpp>
 
 #include "modulo_components/ComponentInterface.hpp"
+#include "modulo_core/concepts.hpp"
 
 namespace modulo_components {
 
@@ -155,6 +156,15 @@ inline void Component::add_output(
                 ->create_publisher_interface(message_pair);
         break;
       }
+      case MessageType::CUSTOM_MESSAGE: {
+        if constexpr (modulo_core::concepts::CustomT<DataT>) {
+          auto publisher = this->create_publisher<DataT>(topic_name, this->get_qos());
+          this->outputs_.at(parsed_signal_name) =
+              std::make_shared<PublisherHandler<rclcpp::Publisher<DataT>, DataT>>(PublisherType::PUBLISHER, publisher)
+                  ->create_publisher_interface(message_pair);
+        }
+        break;
+      }
     }
   } catch (const std::exception& ex) {
     RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what());

source/modulo_components/include/modulo_components/ComponentInterface.hpp

@@ -13,6 +13,7 @@
 #include <modulo_core/communication/PublisherHandler.hpp>
 #include <modulo_core/communication/PublisherType.hpp>
 #include <modulo_core/communication/SubscriptionHandler.hpp>
+#include <modulo_core/concepts.hpp>
 #include <modulo_core/exceptions.hpp>
 #include <modulo_core/translators/parameter_translators.hpp>
 
@@ -694,6 +695,16 @@ inline void ComponentInterface::add_input(
         subscription_interface = subscription_handler->create_subscription_interface(subscription);
         break;
       }
+      case MessageType::CUSTOM_MESSAGE: {
+        if constexpr (modulo_core::concepts::CustomT<DataT>) {
+          auto subscription_handler = std::make_shared<SubscriptionHandler<DataT>>(message_pair);
+          auto subscription = rclcpp::create_subscription<DataT>(
+              this->node_parameters_, this->node_topics_, topic_name, this->qos_,
+              subscription_handler->get_callback(user_callback));
+          subscription_interface = subscription_handler->create_subscription_interface(subscription);
+        }
+        break;
+      }
     }
     this->inputs_.insert_or_assign(parsed_signal_name, subscription_interface);
   } catch (const std::exception& ex) {

source/modulo_components/include/modulo_components/LifecycleComponent.hpp

@@ -273,12 +273,18 @@ class LifecycleComponent : public rclcpp_lifecycle::LifecycleNode, public Compon
   using ComponentInterface::publish_outputs;
   using ComponentInterface::publish_predicates;
   using rclcpp_lifecycle::LifecycleNode::get_parameter;
+
+  std::map<
+      std::string,
+      std::function<std::shared_ptr<modulo_core::communication::PublisherInterface>(const std::string& topic_name)>>
+      configuration_callables_;///< Map of configuration callables
 };
 
 template<typename DataT>
 inline void LifecycleComponent::add_output(
     const std::string& signal_name, const std::shared_ptr<DataT>& data, const std::string& default_topic,
     bool fixed_topic, bool publish_on_step) {
+
   if (this->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED
       && this->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
     RCLCPP_WARN_STREAM_THROTTLE(
@@ -287,9 +293,97 @@ inline void LifecycleComponent::add_output(
     return;
   }
   try {
-    this->create_output(
+    auto parsed_signal_name = this->create_output(
         modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, signal_name, data, default_topic, fixed_topic,
         publish_on_step);
+
+    using modulo_core::communication::MessageType;
+    using modulo_core::communication::PublisherHandler;
+    using modulo_core::communication::PublisherInterface;
+    using modulo_core::communication::PublisherType;
+
+    auto message_pair = this->outputs_.at(parsed_signal_name)->get_message_pair();
+
+    switch (this->outputs_.at(parsed_signal_name)->get_message_pair()->get_type()) {
+      case MessageType::BOOL: {
+        this->configuration_callables_.insert_or_assign(
+            parsed_signal_name, [this, message_pair](const std::string& topic_name) {
+              auto publisher = this->create_publisher<std_msgs::msg::Bool>(topic_name, this->get_qos());
+              return std::make_shared<PublisherHandler<
+                  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Bool>, std_msgs::msg::Bool>>(
+                         PublisherType::LIFECYCLE_PUBLISHER, publisher)
+                  ->create_publisher_interface(message_pair);
+            });
+        break;
+      }
+      case MessageType::FLOAT64: {
+        this->configuration_callables_.insert_or_assign(
+            parsed_signal_name, [this, message_pair](const std::string& topic_name) {
+              auto publisher = this->create_publisher<std_msgs::msg::Float64>(topic_name, this->get_qos());
+              return std::make_shared<PublisherHandler<
+                  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Float64>, std_msgs::msg::Float64>>(
+                         PublisherType::LIFECYCLE_PUBLISHER, publisher)
+                  ->create_publisher_interface(message_pair);
+            });
+        break;
+      }
+      case MessageType::FLOAT64_MULTI_ARRAY: {
+        this->configuration_callables_.insert_or_assign(
+            parsed_signal_name, [this, message_pair](const std::string& topic_name) {
+              auto publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(topic_name, this->get_qos());
+              return std::make_shared<PublisherHandler<
+                  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Float64MultiArray>,
+                  std_msgs::msg::Float64MultiArray>>(PublisherType::LIFECYCLE_PUBLISHER, publisher)
+                  ->create_publisher_interface(message_pair);
+            });
+        break;
+      }
+      case MessageType::INT32: {
+        this->configuration_callables_.insert_or_assign(
+            parsed_signal_name, [this, message_pair](const std::string& topic_name) {
+              auto publisher = this->create_publisher<std_msgs::msg::Int32>(topic_name, this->get_qos());
+              return std::make_shared<PublisherHandler<
+                  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Int32>, std_msgs::msg::Int32>>(
+                         PublisherType::LIFECYCLE_PUBLISHER, publisher)
+                  ->create_publisher_interface(message_pair);
+            });
+        break;
+      }
+      case MessageType::STRING: {
+        this->configuration_callables_.insert_or_assign(
+            parsed_signal_name, [this, message_pair](const std::string& topic_name) {
+              auto publisher = this->create_publisher<std_msgs::msg::String>(topic_name, this->get_qos());
+              return std::make_shared<PublisherHandler<
+                  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>, std_msgs::msg::String>>(
+                         PublisherType::LIFECYCLE_PUBLISHER, publisher)
+                  ->create_publisher_interface(message_pair);
+            });
+        break;
+      }
+      case MessageType::ENCODED_STATE: {
+        this->configuration_callables_.insert_or_assign(
+            parsed_signal_name, [this, message_pair](const std::string& topic_name) {
+              auto publisher = this->create_publisher<modulo_core::EncodedState>(topic_name, this->get_qos());
+              return std::make_shared<PublisherHandler<
+                  rclcpp_lifecycle::LifecyclePublisher<modulo_core::EncodedState>, modulo_core::EncodedState>>(
+                         PublisherType::LIFECYCLE_PUBLISHER, publisher)
+                  ->create_publisher_interface(message_pair);
+            });
+        break;
+      }
+      case MessageType::CUSTOM_MESSAGE: {
+        if constexpr (modulo_core::concepts::CustomT<DataT>) {
+          this->configuration_callables_.insert_or_assign(
+              parsed_signal_name, [this, message_pair](const std::string& topic_name) {
+                auto publisher = this->create_publisher<DataT>(topic_name, this->get_qos());
+                return std::make_shared<PublisherHandler<rclcpp_lifecycle::LifecyclePublisher<DataT>, DataT>>(
+                           PublisherType::LIFECYCLE_PUBLISHER, publisher)
+                    ->create_publisher_interface(message_pair);
+              });
+        }
+        break;
+      }
+    }
   } catch (const modulo_core::exceptions::AddSignalException& ex) {
     RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what());
   }

source/modulo_components/src/LifecycleComponent.cpp

@@ -243,56 +243,7 @@ bool LifecycleComponent::configure_outputs() {
       auto topic_name = this->get_parameter_value<std::string>(name + "_topic");
       RCLCPP_DEBUG_STREAM(
           this->get_logger(), "Configuring output '" << name << "' with topic name '" << topic_name << "'.");
-      auto message_pair = interface->get_message_pair();
-      switch (message_pair->get_type()) {
-        case MessageType::BOOL: {
-          auto publisher = this->create_publisher<std_msgs::msg::Bool>(topic_name, this->get_qos());
-          interface = std::make_shared<PublisherHandler<LifecyclePublisher<std_msgs::msg::Bool>, std_msgs::msg::Bool>>(
-                          PublisherType::LIFECYCLE_PUBLISHER, publisher)
-                          ->create_publisher_interface(message_pair);
-          break;
-        }
-        case MessageType::FLOAT64: {
-          auto publisher = this->create_publisher<std_msgs::msg::Float64>(topic_name, this->get_qos());
-          interface =
-              std::make_shared<PublisherHandler<LifecyclePublisher<std_msgs::msg::Float64>, std_msgs::msg::Float64>>(
-                  PublisherType::LIFECYCLE_PUBLISHER, publisher)
-                  ->create_publisher_interface(message_pair);
-          break;
-        }
-        case MessageType::FLOAT64_MULTI_ARRAY: {
-          auto publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(topic_name, this->get_qos());
-          interface = std::make_shared<PublisherHandler<
-              LifecyclePublisher<std_msgs::msg::Float64MultiArray>, std_msgs::msg::Float64MultiArray>>(
-                          PublisherType::LIFECYCLE_PUBLISHER, publisher)
-                          ->create_publisher_interface(message_pair);
-          break;
-        }
-        case MessageType::INT32: {
-          auto publisher = this->create_publisher<std_msgs::msg::Int32>(topic_name, this->get_qos());
-          interface =
-              std::make_shared<PublisherHandler<LifecyclePublisher<std_msgs::msg::Int32>, std_msgs::msg::Int32>>(
-                  PublisherType::LIFECYCLE_PUBLISHER, publisher)
-                  ->create_publisher_interface(message_pair);
-          break;
-        }
-        case MessageType::STRING: {
-          auto publisher = this->create_publisher<std_msgs::msg::String>(topic_name, this->get_qos());
-          interface =
-              std::make_shared<PublisherHandler<LifecyclePublisher<std_msgs::msg::String>, std_msgs::msg::String>>(
-                  PublisherType::LIFECYCLE_PUBLISHER, publisher)
-                  ->create_publisher_interface(message_pair);
-          break;
-        }
-        case MessageType::ENCODED_STATE: {
-          auto publisher = this->create_publisher<modulo_core::EncodedState>(topic_name, this->get_qos());
-          interface = std::make_shared<
-                          PublisherHandler<LifecyclePublisher<modulo_core::EncodedState>, modulo_core::EncodedState>>(
-                          PublisherType::LIFECYCLE_PUBLISHER, publisher)
-                          ->create_publisher_interface(message_pair);
-          break;
-        }
-      }
+      interface = this->configuration_callables_.at(name)(topic_name);
     } catch (const modulo_core::exceptions::CoreException& ex) {
       success = false;
       RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to configure output '" << name << "': " << ex.what());

source/modulo_components/test/cpp/test_component.cpp

@@ -1,6 +1,8 @@
 #include <gtest/gtest.h>
 
+#include <geometry_msgs/msg/twist.hpp>
 #include <rclcpp/node_options.hpp>
+#include <sensor_msgs/msg/image.hpp>
 
 #include <modulo_core/EncodedState.hpp>
 
@@ -70,5 +72,24 @@ TEST_F(ComponentTest, AddRemoveOutput) {
   EXPECT_NO_THROW(component_->publish_output("8_teEsTt_#1@3"));
   EXPECT_NO_THROW(component_->publish_output("test_13"));
   EXPECT_THROW(component_->publish_output(""), modulo_core::exceptions::CoreException);
+
+  auto std_msg_data = std::make_shared<std_msgs::msg::String>();
+  std_msg_data->data = "foo";
+  component_->add_output("custom_msg_test", std_msg_data);
+  EXPECT_TRUE(component_->outputs_.find("custom_msg_test") != component_->outputs_.end());
+  EXPECT_NO_THROW(component_->outputs_.at("custom_msg_test")->publish());
+  EXPECT_THROW(component_->publish_output("custom_msg_test"), modulo_core::exceptions::CoreException);
+
+  auto geometry_msg_data = std::make_shared<geometry_msgs::msg::Twist>();
+  component_->add_output("geometry_msg_test", geometry_msg_data);
+  EXPECT_TRUE(component_->outputs_.find("geometry_msg_test") != component_->outputs_.end());
+  EXPECT_NO_THROW(component_->outputs_.at("geometry_msg_test")->publish());
+  EXPECT_THROW(component_->publish_output("geometry_msg_test"), modulo_core::exceptions::CoreException);
+
+  auto sensor_msg_data = std::make_shared<sensor_msgs::msg::Image>();
+  component_->add_output("sensor_msg_test", sensor_msg_data);
+  EXPECT_TRUE(component_->outputs_.find("sensor_msg_test") != component_->outputs_.end());
+  EXPECT_NO_THROW(component_->outputs_.at("sensor_msg_test")->publish());
+  EXPECT_THROW(component_->publish_output("sensor_msg_test"), modulo_core::exceptions::CoreException);
 }
 }// namespace modulo_components

source/modulo_components/test/cpp/test_component_interface.cpp

@@ -8,6 +8,8 @@
 
 #include "test_modulo_components/component_public_interfaces.hpp"
 
+#include <sensor_msgs/msg/image.hpp>
+
 namespace modulo_components {
 
 using namespace std::chrono_literals;
@@ -107,6 +109,11 @@ TYPED_TEST(ComponentInterfaceTest, AddRemoveInput) {
   // remove input
   this->component_->remove_input("test_13");
   EXPECT_TRUE(this->component_->inputs_.find("test_13") == this->component_->inputs_.end());
+
+  auto sensor_msg_data = std::make_shared<sensor_msgs::msg::Image>();
+  sensor_msg_data->height = 480;
+  EXPECT_NO_THROW(this->component_->add_input("sensor_msg_data", sensor_msg_data));
+  EXPECT_FALSE(this->component_->inputs_.find("sensor_msg_data") == this->component_->inputs_.end());
 }
 
 TYPED_TEST(ComponentInterfaceTest, AddInputWithUserCallback) {