Always trigger guard condition waitset (ros2#1923)

* trigger guard condition waitset regardless of whether a trigger callback is present

Signed-off-by: Alberto Soragna <[email protected]>

* restore mutex in guard_condition.cpp

Signed-off-by: Alberto Soragna <[email protected]>

* remove whitespace

Signed-off-by: Alberto Soragna <[email protected]>

* add unit-test

Signed-off-by: Alberto Soragna <[email protected]>

* add documentation for trigger and set_on_trigger_callback

Signed-off-by: Alberto Soragna <[email protected]>

Author: alsora

rclcpp/include/rclcpp/guard_condition.hpp

@@ -72,7 +72,7 @@ class GuardCondition
   const rcl_guard_condition_t &
   get_rcl_guard_condition() const;
 
-  /// Notify the wait set waiting on this condition, if any, that the condition had been met.
+  /// Signal that the condition has been met, notifying both the wait set and listeners, if any.
   /**
    * This function is thread-safe, and may be called concurrently with waiting
    * on this guard condition in a wait set.
@@ -107,6 +107,22 @@ class GuardCondition
   void
   add_to_wait_set(rcl_wait_set_t * wait_set);
 
+  /// Set a callback to be called whenever the guard condition is triggered.
+  /**
+   * The callback receives a size_t which is the number of times the guard condition was triggered
+   * since the last time this callback was called.
+   * Normally this is 1, but can be > 1 if the guard condition was triggered before any
+   * callback was set.
+   *
+   * Calling it again will clear any previously set callback.
+   *
+   * This function is thread-safe.
+   *
+   * If you want more information available in the callback, like the guard condition
+   * or other information, you may use a lambda with captures or std::bind.
+   *
+   * \param[in] callback functor to be called when the guard condition is triggered
+   */
   RCLCPP_PUBLIC
   void
   set_on_trigger_callback(std::function<void(size_t)> callback);

rclcpp/src/rclcpp/guard_condition.cpp

@@ -74,16 +74,19 @@ GuardCondition::get_rcl_guard_condition() const
 void
 GuardCondition::trigger()
 {
-  std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+  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);
+  {
+    std::lock_guard<std::recursive_mutex> lock(reentrant_mutex_);
+
+    if (on_trigger_callback_) {
+      on_trigger_callback_(1);
+    } else {
+      unread_count_++;
     }
-    unread_count_++;
   }
 }
 
@@ -125,10 +128,9 @@ GuardCondition::set_on_trigger_callback(std::function<void(size_t)> callback)
       callback(unread_count_);
       unread_count_ = 0;
     }
-    return;
+  } else {
+    on_trigger_callback_ = nullptr;
   }
-
-  on_trigger_callback_ = nullptr;
 }
 
 }  // namespace rclcpp

rclcpp/test/rclcpp/test_guard_condition.cpp

@@ -164,3 +164,21 @@ TEST_F(TestGuardCondition, set_on_trigger_callback) {
     EXPECT_EQ(c1.load(), 2u);
   }
 }
+
+/*
+ * Testing that callback and waitset are both notified by triggering gc
+ */
+TEST_F(TestGuardCondition, callback_and_waitset) {
+  auto gc = std::make_shared<rclcpp::GuardCondition>();
+  std::atomic<size_t> c1 {0};
+  auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;};
+  gc->set_on_trigger_callback(increase_c1_cb);
+
+  rclcpp::WaitSet wait_set;
+  wait_set.add_guard_condition(gc);
+
+  gc->trigger();
+
+  EXPECT_EQ(rclcpp::WaitResultKind::Ready, wait_set.wait(std::chrono::seconds(1)).kind());
+  EXPECT_EQ(c1.load(), 1u);
+}