feat: expose generic outputs to components

source/modulo_components/CMakeLists.txt

@@ -60,4 +60,4 @@ if(BUILD_TESTING)
 
 endif()
 
-ament_auto_package()
+ament_auto_package()

source/modulo_components/include/modulo_components/Component.hpp

@@ -5,9 +5,12 @@
 #include <rclcpp/node.hpp>
 
 #include "modulo_components/ComponentInterface.hpp"
+#include "modulo_core/concepts.hpp"
 
 namespace modulo_components {
 
+using namespace modulo_core::concepts;
+
 /**
  * @class Component
  * @brief A wrapper for rclcpp::Node to simplify application composition through unified component interfaces.
@@ -107,6 +110,14 @@ inline void Component::add_output(
         this->get_logger(), "Adding output '" << parsed_signal_name << "' with topic name '" << topic_name << "'.");
     auto message_pair = this->outputs_.at(parsed_signal_name)->get_message_pair();
     switch (message_pair->get_type()) {
+      case MessageType::ENCODED_STATE: {
+        auto publisher = this->create_publisher<modulo_core::EncodedState>(topic_name, this->get_qos());
+        this->outputs_.at(parsed_signal_name) =
+            std::make_shared<PublisherHandler<rclcpp::Publisher<modulo_core::EncodedState>, modulo_core::EncodedState>>(
+                PublisherType::PUBLISHER, publisher)
+                ->create_publisher_interface(message_pair);
+        break;
+      }
       case MessageType::BOOL: {
         auto publisher = this->create_publisher<std_msgs::msg::Bool>(topic_name, this->get_qos());
         this->outputs_.at(parsed_signal_name) =
@@ -147,17 +158,19 @@ inline void Component::add_output(
                 ->create_publisher_interface(message_pair);
         break;
       }
-      case MessageType::ENCODED_STATE: {
-        auto publisher = this->create_publisher<modulo_core::EncodedState>(topic_name, this->get_qos());
-        this->outputs_.at(parsed_signal_name) =
-            std::make_shared<PublisherHandler<rclcpp::Publisher<modulo_core::EncodedState>, modulo_core::EncodedState>>(
-                PublisherType::PUBLISHER, publisher)
-                ->create_publisher_interface(message_pair);
+      case MessageType::CUSTOM_MESSAGE: {
+        if constexpr (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());
   }
 }
+
 }// namespace modulo_components

source/modulo_components/include/modulo_components/ComponentInterface.hpp

@@ -694,6 +694,10 @@ inline void ComponentInterface::add_input(
         subscription_interface = subscription_handler->create_subscription_interface(subscription);
         break;
       }
+      case MessageType::CUSTOM_MESSAGE: {
+        // TODO:
+        break;
+      }
     }
     this->inputs_.insert_or_assign(parsed_signal_name, subscription_interface);
   } catch (const std::exception& ex) {

source/modulo_components/package.xml

@@ -20,7 +20,7 @@
   <depend>modulo_interfaces</depend>
   <depend>modulo_utils</depend>
   <depend>tf2_ros</depend>
-
+  
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

source/modulo_components/src/LifecycleComponent.cpp

@@ -292,6 +292,8 @@ bool LifecycleComponent::configure_outputs() {
                           ->create_publisher_interface(message_pair);
           break;
         }
+        default:
+          break;
       }
     } catch (const modulo_core::exceptions::CoreException& ex) {
       success = false;

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_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) { // TODO: uncomment when Subscription handler (needed by MinimalSubscriber) supports generic types
+//   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);
+// }

source/modulo_core/test/cpp/communication/test_publisher_handler.cpp

@@ -5,6 +5,9 @@
 #include "modulo_core/communication/MessagePair.hpp"
 #include "modulo_core/communication/PublisherHandler.hpp"
 
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+
 using namespace modulo_core::communication;
 
 template<typename MsgT, typename DataT>
@@ -70,3 +73,8 @@ TEST_F(PublisherTest, EncodedState) {
   publisher_interface = publisher_handler->create_publisher_interface(message_pair);
   EXPECT_NO_THROW(publisher_interface->publish());
 }
+
+TEST_F(PublisherTest, CustomTypes) {
+  test_publisher_interface<sensor_msgs::msg::Image, sensor_msgs::msg::Image>(node, sensor_msgs::msg::Image());
+  test_publisher_interface<sensor_msgs::msg::Imu, sensor_msgs::msg::Imu>(node, sensor_msgs::msg::Imu());
+}