Request to create humble branch

cs4home_core/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.10)
 project(cs4home_core)
 
 set(CMAKE_BUILD_TYPE debug)
@@ -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

@@ -12,143 +12,200 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-
 #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"
+#include "rclcpp/create_generic_subscription.hpp"
 #include "rclcpp/macros.hpp"
+#include "rclcpp/rclcpp.hpp"
 #include "rclcpp/serialization.hpp"
-#include "rclcpp/create_generic_subscription.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
-namespace cs4home_core
-{
+namespace cs4home_core {
 
 /**
  * @class Afferent
- * @brief Manages afferent processing in robotic nodes, including message handling,
- *        subscriptions, and modes for processing serialized data.
+ * @brief Manages afferent processing in robotic nodes, including message
+ * handling, subscriptions, and modes for processing serialized data.
  */
-class Afferent
-{
+class Afferent {
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(Afferent)
 
   /**
-  * @enum EfferentProcessMode
-  * @brief Defines processing modes for serialized message handling.
-  */
-  enum EfferentProcessMode {CALLBACK, ONDEMAND};
+   * @enum AfferentProcessMode
+   * @brief Defines processing modes for handling incoming serialized messages
+   * in an Afferent component.
+   *
+   * - CALLBACK: Incoming messages are processed immediately upon arrival via a
+   * callback function.
+   *
+   * - ONDEMAND: Incoming messages are queued and accessed explicitly through a
+   * `get_msg()` function, allowing controlled and deferred processing.
+   */
+  enum AfferentProcessMode { CALLBACK, ONDEMAND };
 
   /**
    * @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.
+   * @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.
+   * @param cb Optional callback function for handling serialized messages in
+   * CALLBACK mode.
    */
-  void set_mode(
-    EfferentProcessMode mode,
-    std::function<void(std::unique_ptr<rclcpp::SerializedMessage>)> cb = nullptr);
+  void set_mode(const std::string &topic, AfferentProcessMode mode,
+                std::function<void(std::shared_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, AfferentProcessMode mode,
+                std::function<void(std::shared_ptr<rclcpp::SerializedMessage>)>
+                    cb = nullptr);
 
   /**
    * @brief Gets the current processing mode.
-   * @return The current EfferentProcessMode.
+   * @return The current AfferentProcessMode.
    */
-  EfferentProcessMode get_mode() {return mode_;}
+  AfferentProcessMode get_mode() { return mode_; }
 
   /**
    * @brief Sets the maximum queue size for storing messages.
    * @param size Maximum number of messages the queue can hold.
    */
-  void set_max_queue_size(size_t size) {max_queue_size_ = size;}
+  void set_max_queue_size(size_t size) { max_queue_size_ = size; }
 
   /**
    * @brief Gets the maximum queue size.
    * @return The maximum queue size.
    */
-  size_t get_max_queue_size() {return max_queue_size_;}
+  size_t get_max_queue_size() { return max_queue_size_; }
 
   /**
-  * @brief Converts a serialized message to a typed message.
-  * @tparam MessageT Type of the message to deserialize.
-  * @param msg Serialized message to convert.
-  * @return A unique pointer to the deserialized message.
-  */
-  template<class MessageT> std::unique_ptr<MessageT> get_msg(
-    std::unique_ptr<rclcpp::SerializedMessage> msg)
-  {
-    auto typed_msg = std::make_unique<MessageT>();
+   * @brief Converts a serialized message to a typed message.
+   * @tparam MessageT Type of the message to deserialize.
+   * @param msg Serialized message to convert.
+   * @return A shared pointer to the deserialized message.
+   */
+  template <class MessageT>
+  std::shared_ptr<MessageT>
+  get_msg(std::shared_ptr<rclcpp::SerializedMessage> msg) {
+    auto typed_msg = std::make_shared<MessageT>();
     rclcpp::Serialization<MessageT> serializer;
     serializer.deserialize_message(msg.get(), typed_msg.get());
 
-    return std::move(typed_msg);
+    return typed_msg;
   }
 
   /**
    * @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()
-  {
-    if (msg_queue_.empty()) {
-      return {};
+  template <class MessageT>
+  std::shared_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_queue_.front());
-    msg_queue_.pop();
+    std::shared_ptr<rclcpp::SerializedMessage> msg = msg_queues_[topic].front();
+    msg_queues_[topic].pop();
 
-    return get_msg<MessageT>(std::move(msg));
+    return get_msg<MessageT>(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::shared_ptr<MessageT> get_msg(size_t topic_idx) {
+    if (topic_idx >= msg_queues_.size()) {
+      return nullptr;
+    }
+
+    const std::string &topic = input_topic_names_[topic_idx];
+
+    if (msg_queues_.find(topic) == msg_queues_.end() ||
+        msg_queues_[topic].empty()) {
+      return nullptr;
+    }
+
+    std::shared_ptr<rclcpp::SerializedMessage> msg = msg_queues_[topic].front();
+    msg_queues_[topic].pop();
+
+    return get_msg<MessageT>(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. */
+  AfferentProcessMode 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};
+  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::shared_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::shared_ptr<rclcpp::SerializedMessage>)>>
+      callbacks_;
 
   /**
    * @brief Creates a subscriber for a specific topic and message type.
    * @param topic Topic to subscribe to.
    * @param type Type of message for the subscription.
    * @return True if the subscriber was created successfully.
    */
-  bool create_subscriber(const std::string & topic, const std::string & type);
+  bool create_subscriber(const std::string &topic, const std::string &type);
 };
 
-}  // namespace cs4home_core
+} // namespace cs4home_core
 
-#endif  // CS4HOME_CORE__AFFERENT_HPP_
+#endif // CS4HOME_CORE__AFFERENT_HPP_

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