Merge pull request #47 from CoreSenseEU/cs_core_base_2

Afferent, Efferent and Core finished and tested

Author: fmrico

cs4home_core/include/cs4home_core/Afferent.hpp

@@ -17,6 +17,8 @@
 #define CS4HOME_CORE__AFFERENT_HPP_
 
 #include <memory>
+#include <utility>
+#include <queue>
 #include <vector>
 #include <string>
 
@@ -32,16 +34,54 @@ class Afferent
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(Afferent)
 
+  enum EfferentProcessMode {CALLBACK, ONDEMAND};
+
   explicit Afferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
 
-  bool configure();
+  virtual bool configure() = 0;
+
+  void set_mode(
+    EfferentProcessMode mode,
+    std::function<void(std::unique_ptr<rclcpp::SerializedMessage>)> cb = nullptr);
+
+  EfferentProcessMode get_mode() {return mode_;}
+  void set_max_queue_size(size_t size) {max_queue_size_ = size;}
+  size_t get_max_queue_size() {return max_queue_size_;}
+
+  template<class MessageT> std::unique_ptr<MessageT> get_msg(
+    std::unique_ptr<rclcpp::SerializedMessage> msg)
+  {
+    auto typed_msg = std::make_unique<MessageT>();
+    rclcpp::Serialization<MessageT> serializer;
+    serializer.deserialize_message(msg.get(), typed_msg.get());
+
+    return std::move(typed_msg);
+  }
+
+  template<class MessageT> std::unique_ptr<MessageT> get_msg()
+  {
+    if (msg_queue_.empty()) {
+      return {};
+    }
+
+    std::unique_ptr<rclcpp::SerializedMessage> msg = std::move(msg_queue_.front());
+    msg_queue_.pop();
+
+    return get_msg<MessageT>(std::move(msg));
+  }
 
 protected:
   rclcpp_lifecycle::LifecycleNode::SharedPtr parent_;
-
   std::vector<std::shared_ptr<rclcpp::GenericSubscription>> subs_;
 
-private:
+  EfferentProcessMode mode_ {ONDEMAND};
+
+  const size_t MAX_DEFAULT_QUEUE_SIZE = 100;
+  size_t max_queue_size_ {MAX_DEFAULT_QUEUE_SIZE};
+  std::queue<std::unique_ptr<rclcpp::SerializedMessage>> msg_queue_;
+
+  std::function<void(std::unique_ptr<rclcpp::SerializedMessage>)> callback_;
+
   bool create_subscriber(const std::string & topic, const std::string & type);
 };
 

cs4home_core/include/cs4home_core/Core.hpp

@@ -18,6 +18,10 @@
 
 #include <memory>
 
+
+#include "cs4home_core/Afferent.hpp"
+#include "cs4home_core/Efferent.hpp"
+
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp/macros.hpp"
 
@@ -31,10 +35,17 @@ class Core
 
   explicit Core(rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
 
-  bool configure();
+  virtual bool configure() = 0;
+  virtual bool activate() = 0;
+  virtual bool deactivate() = 0;
+
+  void set_afferent(cs4home_core::Afferent::SharedPtr afferent) {afferent_ = afferent;}
+  void set_efferent(cs4home_core::Efferent::SharedPtr efferent) {efferent_ = efferent;}
 
 protected:
   rclcpp_lifecycle::LifecycleNode::SharedPtr parent_;
+  cs4home_core::Afferent::SharedPtr afferent_;
+  cs4home_core::Efferent::SharedPtr efferent_;
 };
 
 }  // namespace cs4home_core

cs4home_core/include/cs4home_core/Efferent.hpp

@@ -16,6 +16,10 @@
 #ifndef CS4HOME_CORE__EFFERENT_HPP_
 #define CS4HOME_CORE__EFFERENT_HPP_
 
+#include <memory>
+#include <string>
+#include <vector>
+
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp/macros.hpp"
 
@@ -29,10 +33,26 @@ class Efferent
 
   explicit Efferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent);
 
-  bool configure();
+  virtual bool configure() = 0;
+
+  template<class MessageT>
+  void publish(std::unique_ptr<MessageT> msg)
+  {
+    rclcpp::Serialization<MessageT> serializer;
+    auto untyped_msg = rclcpp::SerializedMessage();
+
+    serializer.serialize_message(msg.get(), &untyped_msg);
+
+    for (auto & pub : pubs_) {
+      pub->publish(untyped_msg);
+    }
+  }
 
 protected:
   rclcpp_lifecycle::LifecycleNode::SharedPtr parent_;
+  std::vector<std::shared_ptr<rclcpp::GenericPublisher>> pubs_;
+
+  bool create_publisher(const std::string & topic, const std::string & type);
 };
 
 }  // namespace cs4home_core

cs4home_core/src/cs4home_core/Afferent.cpp

@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+
 #include "cs4home_core/Afferent.hpp"
 
-#include "rclcpp/node.hpp"
-#include "rclcpp/serialization.hpp"
+#include "rclcpp/rclcpp.hpp"
 
 namespace cs4home_core
 {
@@ -25,38 +25,48 @@ Afferent::Afferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent)
 {
 }
 
-bool
-Afferent::configure()
+void
+Afferent::set_mode(
+  EfferentProcessMode mode,
+  std::function<void(std::unique_ptr<rclcpp::SerializedMessage>)> cb)
 {
-  /*std::vector<std::string> afferents;
-  parent_->declare_parameter("afferent", afferents);
-  parent_->get_parameter("afferent", afferents);
-
-  for (const auto & afferent: afferents) {
-    std::string topic, type;
-    parent_->declare_parameter("afferent." + afferent + ".topic", topic);
-    parent_->get_parameter("afferent." + afferent + ".type", type);
-
-    if (topic == "" || type == "") {
-      return false;
-    }
-
-    if (!create_subscriber(topic, type)) {
-      RCLCPP_ERROR(
-        parent_->get_logger(), "Error creating afferent [%s, %s]", topic.c_str(), type.c_str());
-      return false;
+  if (mode == CALLBACK) {
+    if (cb) {
+      callback_ = cb;
+    } else {
+      RCLCPP_WARN(
+        parent_->get_logger(), "[Afferent] Error setting callback: not function specified");
+      return;
     }
   }
-*/
-  return true;
+  mode_ = mode;
 }
 
 bool
 Afferent::create_subscriber(const std::string & topic, const std::string & type)
 {
-  auto sub = parent_->create_generic_subscription(topic, type, 100,
-      [&](std::shared_ptr<rclcpp::SerializedMessage> msg) {
-        RCLCPP_INFO(parent_->get_logger(), "Llega el mensaje");
+  RCLCPP_DEBUG(
+    parent_->get_logger(),
+    "[Afferent] Creating subscription [%s, %s]",
+    topic.c_str(), type.c_str());
+
+  auto sub = rclcpp::create_generic_subscription(
+    parent_->get_node_topics_interface(), topic, type, 100,
+    [&](std::unique_ptr<rclcpp::SerializedMessage> msg)
+    {
+      if (mode_ == CALLBACK) {
+        if (callback_) {
+          callback_(std::move(msg));
+        } else {
+          RCLCPP_WARN(
+            parent_->get_logger(), "[Afferent] Error calling callback: not function specified");
+        }
+      } else {
+        msg_queue_.push(std::move(msg));
+        if (msg_queue_.size() > max_queue_size_) {
+          msg_queue_.pop();
+        }
+      }
     });
 
   subs_.push_back(sub);

cs4home_core/src/cs4home_core/CognitiveModule.cpp

@@ -34,7 +34,8 @@ CallbackReturnT CognitiveModule::on_configure(const rclcpp_lifecycle::State & st
   (void)state;
 
   get_parameter("core", core_name_);
-  auto [core_, error_core] = load_component<Core>(core_name_, shared_from_this());
+  std::string error_core;
+  std::tie(core_, error_core) = load_component<Core>(core_name_, shared_from_this());
   if (core_ == nullptr || !core_->configure()) {
     RCLCPP_ERROR(
       get_logger(), "Error configuring core at %s with name %s: %s",
@@ -43,7 +44,9 @@ CallbackReturnT CognitiveModule::on_configure(const rclcpp_lifecycle::State & st
   }
 
   get_parameter("efferent", efferent_name_);
-  auto [efferent_, error_efferent] = load_component<Efferent>(efferent_name_, shared_from_this());
+  std::string error_efferent;
+  std::tie(efferent_, error_efferent) = load_component<Efferent>(efferent_name_,
+    shared_from_this());
   if (efferent_ == nullptr || !efferent_->configure()) {
     RCLCPP_ERROR(
       get_logger(), "Error configuring efferent at %s with name %s: %s",
@@ -52,16 +55,22 @@ CallbackReturnT CognitiveModule::on_configure(const rclcpp_lifecycle::State & st
   }
 
   get_parameter("afferent", afferent_name_);
-  auto [afferent_, error_afferent] = load_component<Afferent>(afferent_name_, shared_from_this());
+  std::string error_afferent;
+  std::tie(afferent_, error_afferent) = load_component<Afferent>(afferent_name_,
+    shared_from_this());
   if (afferent_ == nullptr || !afferent_->configure()) {
     RCLCPP_ERROR(
       get_logger(), "Error configuring afferent at %s with name %s: %s",
       get_name(), afferent_name_.c_str(), error_afferent.c_str());
     return CallbackReturnT::FAILURE;
   }
 
+  core_->set_afferent(afferent_);
+  core_->set_efferent(efferent_);
+
   get_parameter("meta", meta_name_);
-  auto [meta_, error_meta] = load_component<Meta>(meta_name_, shared_from_this());
+  std::string error_meta;
+  std::tie(meta_, error_meta) = load_component<Meta>(meta_name_, shared_from_this());
   if (meta_ == nullptr || !meta_->configure()) {
     RCLCPP_ERROR(
       get_logger(), "Error configuring efferent at %s with name %s: %s",
@@ -70,7 +79,9 @@ CallbackReturnT CognitiveModule::on_configure(const rclcpp_lifecycle::State & st
   }
 
   get_parameter("coupling", coupling_name_);
-  auto [coupling_, error_coupling] = load_component<Coupling>(coupling_name_, shared_from_this());
+  std::string error_coupling;
+  std::tie(coupling_, error_coupling) = load_component<Coupling>(coupling_name_,
+    shared_from_this());
   if (coupling_ == nullptr || !coupling_->configure()) {
     RCLCPP_ERROR(
       get_logger(), "Error configuring efferent at %s with name %s: %s",
@@ -85,13 +96,23 @@ CallbackReturnT CognitiveModule::on_activate(const rclcpp_lifecycle::State & sta
 {
   (void)state;
 
+  if (!core_->activate()) {
+    RCLCPP_ERROR(get_logger(), "Unable to activate Core");
+    return CallbackReturnT::FAILURE;
+  }
+
   return CallbackReturnT::SUCCESS;
 }
 
 CallbackReturnT CognitiveModule::on_deactivate(const rclcpp_lifecycle::State & state)
 {
   (void)state;
 
+  if (!core_->deactivate()) {
+    RCLCPP_ERROR(get_logger(), "Unable to activate Core");
+    return CallbackReturnT::FAILURE;
+  }
+
   return CallbackReturnT::SUCCESS;
 }
 

cs4home_core/src/cs4home_core/Efferent.cpp

@@ -23,8 +23,13 @@ Efferent::Efferent(rclcpp_lifecycle::LifecycleNode::SharedPtr parent)
 }
 
 bool
-Efferent::configure()
+Efferent::create_publisher(const std::string & topic, const std::string & type)
 {
+  auto pub = rclcpp::create_generic_publisher(
+    parent_->get_node_topics_interface(), topic, type, 100);
+
+  pubs_.push_back(pub);
+
   return true;
 }
 

cs4home_core/test/CMakeLists.txt

@@ -12,6 +12,10 @@ add_library(image_filter SHARED ImageFilter.cpp)
 ament_target_dependencies(image_filter ${dependencies} ${test_dependencies})
 target_link_libraries(image_filter ${PROJECT_NAME})
 
+add_library(image_filter_cb SHARED ImageFilterCB.cpp)
+ament_target_dependencies(image_filter_cb ${dependencies} ${test_dependencies})
+target_link_libraries(image_filter_cb ${PROJECT_NAME})
+
 add_library(simple_image_input SHARED SimpleImageInput.cpp)
 ament_target_dependencies(simple_image_input ${dependencies} ${test_dependencies})
 target_link_libraries(simple_image_input ${PROJECT_NAME})
@@ -31,6 +35,7 @@ target_link_libraries(default_coupling ${PROJECT_NAME})
 
 install(TARGETS
   image_filter
+  image_filter_cb
   simple_image_input
   simple_image_output
   default_meta

cs4home_core/test/DefaultCoupling.cpp

@@ -27,13 +27,13 @@ class DefaultCoupling : public cs4home_core::Coupling
   explicit DefaultCoupling(rclcpp_lifecycle::LifecycleNode::SharedPtr parent)
   : Coupling(parent)
   {
-    RCLCPP_INFO(parent_->get_logger(), "Coupling created: [DefaultCoupling]");
+    RCLCPP_DEBUG(parent_->get_logger(), "Coupling created: [DefaultCoupling]");
   }
 
 
   bool configure()
   {
-    RCLCPP_INFO(parent_->get_logger(), "Coupling configured");
+    RCLCPP_DEBUG(parent_->get_logger(), "Coupling configured");
     return true;
   }
 };

cs4home_core/test/DefaultMeta.cpp

@@ -27,13 +27,13 @@ class DefaultMeta : public cs4home_core::Meta
   explicit DefaultMeta(rclcpp_lifecycle::LifecycleNode::SharedPtr parent)
   : Meta(parent)
   {
-    RCLCPP_INFO(parent_->get_logger(), "Meta created: [DefaultMeta]");
+    RCLCPP_DEBUG(parent_->get_logger(), "Meta created: [DefaultMeta]");
   }
 
 
   bool configure()
   {
-    RCLCPP_INFO(parent_->get_logger(), "Meta configured");
+    RCLCPP_DEBUG(parent_->get_logger(), "Meta configured");
     return true;
   }
 };