[WIP] Improvements in core

cs4home_core/CMakeLists.txt

@@ -11,11 +11,13 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_cascade_lifecycle REQUIRED)
 find_package(rclcpp_components REQUIRED)
 
 set(dependencies
   rclcpp
   rclcpp_lifecycle
+  rclcpp_cascade_lifecycle
   rclcpp_components
 )
 

cs4home_core/include/cs4home_core/Afferent.hpp

@@ -16,11 +16,12 @@
 #ifndef CS4HOME_CORE__AFFERENT_HPP_
 #define CS4HOME_CORE__AFFERENT_HPP_
 
+#include <map>
 #include <memory>
 #include <utility>
 #include <queue>
-#include <vector>
 #include <string>
+#include <vector>
 
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -51,24 +52,37 @@ class Afferent
    * @brief Constructor for the Afferent class.
    * @param parent Shared pointer to the lifecycle node managing this instance.
    */
-  explicit Afferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
+  explicit Afferent(const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
 
   /**
    * @brief Configures the afferent component; intended for subclass implementation.
    * @return True if configuration is successful.
    */
-  virtual bool configure() = 0;
+  virtual bool configure();
 
   /**
    * @brief Sets the processing mode and an optional callback function.
    *
+   * @param topic The topic to subscribe
    * @param mode Processing mode for handling messages.
    * @param cb Optional callback function for handling serialized messages in CALLBACK mode.
    */
   void set_mode(
+    const std::string & topic,
     EfferentProcessMode mode,
     std::function<void(std::unique_ptr<rclcpp::SerializedMessage>)> cb = nullptr);
 
+  /**
+   * @brief Sets the processing mode and an optional callback function.
+   *
+   * @param topic_idx The index of the topic input.
+   * @param mode Processing mode for handling messages.
+   * @param cb Optional callback function for handling serialized messages in CALLBACK mode.
+   */
+  void set_mode(
+    size_t topic_idx,
+    EfferentProcessMode mode,
+    std::function<void(std::unique_ptr<rclcpp::SerializedMessage>)> cb = nullptr);
 
   /**
    * @brief Gets the current processing mode.
@@ -107,37 +121,70 @@ class Afferent
   /**
    * @brief Retrieves the next message from the queue, if available.
    * @tparam MessageT Type of message to retrieve.
-   * @return A unique pointer to the next message, or nullptr if the queue is empty.
+   * @tparam topic The topic of the associated queue to retrieve the message.
+   * @return A unique pointer to the next message, or nullptr if the queue is
+   * empty or does not exists.
+   */
+  template<class MessageT> std::unique_ptr<MessageT> get_msg(const std::string & topic)
+  {
+    if (msg_queues_.find(topic) == msg_queues_.end() || msg_queues_[topic].empty()) {
+      return nullptr;
+    }
+
+    std::unique_ptr<rclcpp::SerializedMessage> msg = std::move(msg_queues_[topic].front());
+    msg_queues_[topic].pop();
+
+    return get_msg<MessageT>(std::move(msg));
+  }
+
+  /**
+   * @brief Retrieves the next message from the queue, if available.
+   * @tparam MessageT Type of message to retrieve.
+   * @tparam topic_idx The index fn the topic input associated queue to retrieve the message.
+   * @return A unique pointer to the next message, or nullptr if the queue is
+   * empty or does not exists.
    */
-  template<class MessageT> std::unique_ptr<MessageT> get_msg()
+  template<class MessageT> std::unique_ptr<MessageT> get_msg(size_t topic_idx)
   {
-    if (msg_queue_.empty()) {
-      return {};
+    if (topic_idx >= msg_queues_.size()) {
+      return nullptr;
     }
 
-    std::unique_ptr<rclcpp::SerializedMessage> msg = std::move(msg_queue_.front());
-    msg_queue_.pop();
+    const std::string & topic = input_topic_names_[topic_idx];
+
+    if (msg_queues_.find(topic) == msg_queues_.end() || msg_queues_[topic].empty()) {
+      return nullptr;
+    }
+
+    std::unique_ptr<rclcpp::SerializedMessage> msg = std::move(msg_queues_[topic].front());
+    msg_queues_[topic].pop();
 
     return get_msg<MessageT>(std::move(msg));
   }
 
 protected:
   /** Shared pointer to the parent node. */
   rclcpp_lifecycle::LifecycleNode::SharedPtr parent_;
-  /** List of subscriptions. */
-  std::vector<std::shared_ptr<rclcpp::GenericSubscription>> subs_;
+  std::string name_;
 
   EfferentProcessMode mode_ {ONDEMAND}; /**< Current processing mode. */
 
   /** Default maximum queue size. */
   const size_t MAX_DEFAULT_QUEUE_SIZE = 100;
   /** Maximum queue size. */
   size_t max_queue_size_ {MAX_DEFAULT_QUEUE_SIZE};
+
+  /** List of subscriptions. */
+  std::vector<std::shared_ptr<rclcpp::GenericSubscription>> subs_;
   /** Queue for serialized messages. */
-  std::queue<std::unique_ptr<rclcpp::SerializedMessage>> msg_queue_;
+  std::map<std::string, std::queue<std::unique_ptr<rclcpp::SerializedMessage>>> msg_queues_;
+  /**< List of input topics to subscribe to for images. */
+  std::vector<std::string> input_topic_names_;
+  /**< List of input topics types. */
+  std::vector<std::string> input_topic_types_;
 
   /** Callback for serialized messages. */
-  std::function<void(std::unique_ptr<rclcpp::SerializedMessage>)> callback_;
+  std::map<std::string, std::function<void(std::unique_ptr<rclcpp::SerializedMessage>)>> callbacks_;
 
 
   /**

cs4home_core/include/cs4home_core/CognitiveModule.hpp

@@ -28,16 +28,17 @@
 #include "rclcpp/macros.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 namespace cs4home_core
 {
 
+class CognitiveModule : public rclcpp_cascade_lifecycle::CascadeLifecycleNode
 /**
  * @class CognitiveModule
- * @brief Extends the LifecycleNode to manage cognitive processing components in a ROS 2 lifecycle,
+ * @brief Extends the Cascade LifecycleNode to manage cognitive processing components in a ROS 2 lifecycle,
  *        including afferent, efferent, core, meta, and coupling components.
  */
-class CognitiveModule : public rclcpp_lifecycle::LifecycleNode
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(CognitiveModule)

cs4home_core/include/cs4home_core/Core.hpp

@@ -16,6 +16,7 @@
 #define CS4HOME_CORE__CORE_HPP_
 
 #include <memory>
+#include <string>
 
 #include "cs4home_core/Afferent.hpp"
 #include "cs4home_core/Efferent.hpp"
@@ -40,7 +41,8 @@ class Core
    * @brief Constructs a Core object associated with a parent lifecycle node.
    * @param parent Shared pointer to the lifecycle node managing this Core instance.
    */
-  explicit Core(rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
+  explicit Core(
+    const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
 
   /**
    * @brief Configures the Core component.
@@ -75,6 +77,8 @@ class Core
 protected:
   /** Shared pointer to the parent lifecycle node. */
   rclcpp_lifecycle::LifecycleNode::SharedPtr parent_;
+
+  std::string name_;
   /** Shared pointer to the Afferent component. */
   cs4home_core::Afferent::SharedPtr afferent_;
   /** Shared pointer to the Efferent component. */

cs4home_core/include/cs4home_core/Coupling.hpp

@@ -15,6 +15,8 @@
 #ifndef CS4HOME_CORE__COUPLING_HPP_
 #define CS4HOME_CORE__COUPLING_HPP_
 
+#include <string>
+
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp/macros.hpp"
 
@@ -35,7 +37,8 @@ class Coupling
    * @brief Constructs a Coupling object associated with a parent lifecycle node.
    * @param parent Shared pointer to the lifecycle node managing this Coupling instance.
    */
-  explicit Coupling(rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
+  explicit Coupling(
+    const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
 
   /**
    * @brief Configures the Coupling component.
@@ -46,6 +49,7 @@ class Coupling
 protected:
   /**< Shared pointer to the parent lifecycle node. */
   rclcpp_lifecycle::LifecycleNode::SharedPtr parent_;
+  std::string name_;
 };
 
 }  // namespace cs4home_core

cs4home_core/include/cs4home_core/Efferent.hpp

@@ -38,15 +38,16 @@ class Efferent
 
   /**
    * @brief Constructs an Efferent object associated with a parent lifecycle node.
+   * @param name name of the component.
    * @param parent Shared pointer to the lifecycle node managing this Efferent instance.
    */
-  explicit Efferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
+  explicit Efferent(const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
 
   /**
    * @brief Configures the Efferent component.
    * @return True if configuration is successful.
    */
-  virtual bool configure() = 0;
+  virtual bool configure();
 
   /**
    * @brief Publishes a serialized message to all configured publishers.
@@ -58,24 +59,35 @@ class Efferent
    * @param msg Unique pointer to the message to broadcast.
    */
   template<class MessageT>
-  void publish(std::unique_ptr<MessageT> msg)
+  void publish(size_t topic_index, std::unique_ptr<MessageT> msg)
   {
+    if (topic_index >= pubs_.size()) {
+      RCLCPP_WARN(
+        parent_->get_logger(), "[Efferent] Error publishing: topic index not valid");
+      return;
+    }
+
     rclcpp::Serialization<MessageT> serializer;
     auto untyped_msg = rclcpp::SerializedMessage();
 
     serializer.serialize_message(msg.get(), &untyped_msg);
 
-    for (auto & pub : pubs_) {
-      pub->publish(untyped_msg);
-    }
+    pubs_[topic_index]->publish(untyped_msg);
   }
 
 protected:
   /**< Shared pointer to the parent lifecycle node. */
   rclcpp_lifecycle::LifecycleNode::SharedPtr parent_;
+  std::string name_;
+
   /**< List of generic publishers. */
   std::vector<std::shared_ptr<rclcpp::GenericPublisher>> pubs_;
 
+  /**< List of output topics to publish images. */
+  std::vector<std::string> output_topic_names_;
+  /**< List of output topics types. */
+  std::vector<std::string> output_topic_types_;
+
   /**
    * @brief Creates a publisher for a specified topic and message type.
    * @param topic The topic name to publish messages to.

cs4home_core/include/cs4home_core/Flow.hpp

@@ -22,6 +22,8 @@
 
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 namespace cs4home_core
 {
@@ -31,7 +33,7 @@ namespace cs4home_core
  * @brief Represents a sequence of nodes within a robotic system, with utilities
  *        to manage and print the node sequence.
  */
-class Flow
+class Flow : public rclcpp_cascade_lifecycle::CascadeLifecycleNode
 {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(Flow)
@@ -40,18 +42,22 @@ class Flow
    * @brief Constructs a Flow object with a specified sequence of nodes.
    * @param nodes A vector of node names to initialize the flow sequence.
    */
-  explicit Flow(const std::vector<std::string> & nodes);
+  explicit Flow(const std::string & name);
+  Flow(const std::string & name, const std::vector<std::string> & nodes);
 
   /**
    * @brief Prints the sequence of nodes in the flow to the standard output.
    */
   void print() const;
-
   /**
    * @brief Retrieves the sequence of nodes in the flow.
    * @return A constant reference to the vector of node names.
    */
-  const std::vector<std::string> & get_nodes() const {return nodes_;}
+  const std::vector<std::string> & get_flow() const {return nodes_;}
+  void set_flow(const std::vector<std::string> & nodes);
+
+  void activate();
+  void deactivate();
 
 private:
   std::vector<std::string> nodes_; /**< Sequence of nodes in the flow. */

cs4home_core/include/cs4home_core/Master.hpp

@@ -17,8 +17,10 @@
 
 #include <map>
 #include <string>
+#include <vector>
 
 #include "cs4home_core/CognitiveModule.hpp"
+#include "cs4home_core/Flow.hpp"
 
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -39,10 +41,12 @@ class Master : public rclcpp_lifecycle::LifecycleNode
   using CallbackReturnT = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
   /**
-   * @brief Constructs a Master lifecycle node with the specified options.
+   * @brief Constructs a Master cascade lifecycle node with the specified options.
    * @param options Node options to configure the Master node.
    */
-  explicit Master(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  explicit Master(
+    const std::string & name,
+    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
   /**
    * @brief Configures the Master node.
@@ -86,9 +90,10 @@ class Master : public rclcpp_lifecycle::LifecycleNode
    */
   CallbackReturnT on_error(const rclcpp_lifecycle::State & state);
 
+  const std::map<std::string, Flow::SharedPtr> & get_flows() const {return flows_;}
+
 protected:
-  /** Map of cognitive modules managed by the Master node. */
-  std::map<std::string, cs4home_core::CognitiveModule::SharedPtr> cog_modules_;
+  std::map<std::string, Flow::SharedPtr> flows_;
 };
 
 }  // namespace cs4home_core

cs4home_core/include/cs4home_core/Meta.hpp

@@ -15,6 +15,8 @@
 #ifndef CS4HOME_CORE__META_HPP_
 #define CS4HOME_CORE__META_HPP_
 
+#include <string>
+
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp/macros.hpp"
 
@@ -35,7 +37,7 @@ class Meta
    * @brief Constructs a Meta object associated with a parent lifecycle node.
    * @param parent Shared pointer to the lifecycle node managing this Meta instance.
    */
-  explicit Meta(rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
+  explicit Meta(const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
 
   /**
    * @brief Configures the Meta component.
@@ -46,6 +48,7 @@ class Meta
 protected:
   /// < Shared pointer to the parent lifecycle node.
   rclcpp_lifecycle::LifecycleNode::SharedPtr parent_;
+  std::string name_;
 };
 
 }  // namespace cs4home_core

cs4home_core/package.xml

@@ -10,6 +10,7 @@
 
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>rclcpp_cascade_lifecycle</depend>
   <depend>rclcpp_components</depend>
 
   <test_depend>ament_lint_auto</test_depend>