diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index f6f5af9586..350f306010 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/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 callback); diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index ea68c78d73..627644e602 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -74,16 +74,19 @@ GuardCondition::get_rcl_guard_condition() const void GuardCondition::trigger() { - std::lock_guard 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 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 callback) callback(unread_count_); unread_count_ = 0; } - return; + } else { + on_trigger_callback_ = nullptr; } - - on_trigger_callback_ = nullptr; } } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_guard_condition.cpp b/rclcpp/test/rclcpp/test_guard_condition.cpp index 481051ccf9..1e72264869 100644 --- a/rclcpp/test/rclcpp/test_guard_condition.cpp +++ b/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(); + std::atomic 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); +}