IPC Subscription to have callback

Signed-off-by: Mauro Passerino <[email protected]>

Author: Mauro Passerino

rclcpp/include/rclcpp/executors/events_executor_entities_collector.hpp

@@ -209,10 +209,10 @@ class EventsExecutorEntitiesCollector final
   unset_callback_group_entities_callbacks(rclcpp::CallbackGroup::SharedPtr group);
 
   void
-  set_guard_condition_callback(const rclcpp::GuardCondition * guard_condition);
+  set_guard_condition_callback(rclcpp::GuardCondition * guard_condition);
 
   void
-  unset_guard_condition_callback(const rclcpp::GuardCondition * guard_condition);
+  unset_guard_condition_callback(rclcpp::GuardCondition * guard_condition);
 
   std::function<void(size_t)>
   create_entity_callback(void * exec_entity_id, ExecutorEventType type);
@@ -249,7 +249,7 @@ class EventsExecutorEntitiesCollector final
   WeakCallbackGroupsToNodesMap weak_groups_to_nodes_associated_with_executor_;
 
   typedef std::map<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr,
-      const rclcpp::GuardCondition *,
+      rclcpp::GuardCondition *,
       std::owner_less<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>>
     WeakNodesToGuardConditionsMap;
   WeakNodesToGuardConditionsMap weak_nodes_to_guard_conditions_;

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -148,7 +148,12 @@ class SubscriptionIntraProcess : public SubscriptionIntraProcessBase
   void
   trigger_guard_condition()
   {
-    gc_.trigger();
+    if (on_new_message_callback_) {
+      on_new_message_callback_(1);
+    } else {
+      gc_.trigger();
+      unread_count_++;
+    }
   }
 
   template<typename T>

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -84,6 +84,8 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
 protected:
   std::recursive_mutex reentrant_mutex_;
   rclcpp::GuardCondition gc_;
+  std::function<void(size_t)> on_new_message_callback_{nullptr};
+  size_t unread_count_{0};
 
 private:
   virtual void

rclcpp/include/rclcpp/guard_condition.hpp

@@ -100,10 +100,16 @@ class GuardCondition
   void
   add_to_wait_set(rcl_wait_set_t * wait_set) const;
 
+  RCLCPP_PUBLIC
+  void
+  set_on_trigger_callback(std::function<void(size_t, int)> callback);
+
 protected:
   rclcpp::Context::SharedPtr context_;
   rcl_guard_condition_t rcl_guard_condition_;
   std::atomic<bool> in_use_by_wait_set_{false};
+  std::function<void(size_t)> on_trigger_callback_{nullptr};
+  size_t unread_count_{0};
 };
 
 }  // namespace rclcpp

rclcpp/src/rclcpp/executors/events_executor_entities_collector.cpp

@@ -67,7 +67,7 @@ EventsExecutorEntitiesCollector::~EventsExecutorEntitiesCollector()
   for (const auto & pair : weak_nodes_to_guard_conditions_) {
     auto node = pair.first.lock();
     if (node) {
-      auto node_gc = pair.second;
+      auto & node_gc = pair.second;
       unset_guard_condition_callback(node_gc);
     }
   }
@@ -267,8 +267,8 @@ EventsExecutorEntitiesCollector::set_callback_group_entities_callbacks(
       if (waitable) {
         weak_waitables_map_.emplace(waitable.get(), waitable);
 
-        // waitable->set_listener_callback(
-        //   create_entity_callback(waitable.get(), WAITABLE_EVENT));
+        waitable->set_listener_callback(
+          create_waitable_callback(waitable.get()));
       }
       return false;
     });
@@ -476,15 +476,16 @@ EventsExecutorEntitiesCollector::get_automatically_added_callback_groups_from_no
 
 void
 EventsExecutorEntitiesCollector::set_guard_condition_callback(
-  const rclcpp::GuardCondition * guard_condition)
+  rclcpp::GuardCondition * guard_condition)
 {
-  create_waitable_callback(this);
+  guard_condition->set_on_trigger_callback(create_waitable_callback(this));
 }
 
 void
 EventsExecutorEntitiesCollector::unset_guard_condition_callback(
-  const rclcpp::GuardCondition * guard_condition)
+  rclcpp::GuardCondition * guard_condition)
 {
+   guard_condition->set_on_trigger_callback(nullptr);
 }
 
 rclcpp::SubscriptionBase::SharedPtr

rclcpp/src/rclcpp/guard_condition.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <functional>
+
 #include "rclcpp/guard_condition.hpp"
 
 #include "rclcpp/exceptions.hpp"
@@ -66,9 +68,14 @@ GuardCondition::get_rcl_guard_condition() const
 void
 GuardCondition::trigger()
 {
-  rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_);
-  if (RCL_RET_OK != ret) {
-    rclcpp::exceptions::throw_from_rcl_error(ret);
+  if (on_trigger_callback_) {
+    on_trigger_callback_(1);
+  } else {
+    rcl_ret_t ret = rcl_trigger_guard_condition(&rcl_guard_condition_);
+    if (RCL_RET_OK != ret) {
+      rclcpp::exceptions::throw_from_rcl_error(ret);
+    }
+    unread_count_++;
   }
 }
 
@@ -87,4 +94,15 @@ GuardCondition::add_to_wait_set(rcl_wait_set_t * wait_set) const
       ret, "failed to add guard condition to wait set");
   }
 }
+
+void
+GuardCondition::set_on_trigger_callback(std::function<void(size_t, int)> callback)
+{
+  on_trigger_callback_ = std::bind(callback, std::placeholders::_1, -1);
+
+  if (unread_count_) {
+    on_trigger_callback_(unread_count_);
+    unread_count_ = 0;
+  }
+}
 }  // namespace rclcpp

rclcpp/src/rclcpp/subscription_intra_process_base.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <functional>
+
 #include "rclcpp/experimental/subscription_intra_process_base.hpp"
 
 using rclcpp::experimental::SubscriptionIntraProcessBase;
@@ -41,5 +43,10 @@ SubscriptionIntraProcessBase::get_actual_qos() const
 void
 SubscriptionIntraProcessBase::set_listener_callback(std::function<void(size_t, int)> callback)
 {
-  (void)callback;
+  on_new_message_callback_ = std::bind(callback, std::placeholders::_1, -1);
+
+  if (unread_count_ && on_new_message_callback_) {
+    on_new_message_callback_(unread_count_);
+    unread_count_ = 0;
+  }
 }