From 28aadf549302191910b1a11e64c260f3e74a58a1 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Thu, 11 Mar 2021 16:12:38 +0000 Subject: [PATCH 01/31] Add RMW listener APIs Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/client.hpp | 6 ++++++ .../subscription_intra_process_base.hpp | 6 ++++++ rclcpp/include/rclcpp/qos_event.hpp | 6 ++++++ rclcpp/include/rclcpp/service.hpp | 6 ++++++ rclcpp/include/rclcpp/subscription_base.hpp | 6 ++++++ rclcpp/include/rclcpp/waitable.hpp | 7 +++++++ rclcpp/src/rclcpp/client.cpp | 15 +++++++++++++++ rclcpp/src/rclcpp/qos_event.cpp | 16 ++++++++++++++++ rclcpp/src/rclcpp/service.cpp | 15 +++++++++++++++ rclcpp/src/rclcpp/subscription_base.cpp | 18 +++++++++++++++++- .../rclcpp/subscription_intra_process_base.cpp | 16 ++++++++++++++++ rclcpp/src/rclcpp/waitable.cpp | 14 ++++++++++++++ 12 files changed, 130 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 1fc761f491..6d50e88cfd 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -215,6 +215,12 @@ class ClientBase bool exchange_in_use_by_wait_set_state(bool in_use_state); + RCLCPP_PUBLIC + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const; + protected: RCLCPP_DISABLE_COPY(ClientBase) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index f0ee39ce47..a0bdddaf0e 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -69,6 +69,12 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable QoS get_actual_qos() const; + RCLCPP_PUBLIC + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const override; + protected: std::recursive_mutex reentrant_mutex_; rclcpp::GuardCondition gc_; diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 04231e1395..6263323e6d 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -102,6 +102,12 @@ class QOSEventHandlerBase : public Waitable bool is_ready(rcl_wait_set_t * wait_set) override; + RCLCPP_PUBLIC + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const override; + protected: rcl_event_t event_handle_; size_t wait_set_event_index_; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 78fc5e048f..b427cc98e4 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -121,6 +121,12 @@ class ServiceBase bool exchange_in_use_by_wait_set_state(bool in_use_state); + RCLCPP_PUBLIC + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const; + protected: RCLCPP_DISABLE_COPY(ServiceBase) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 72ddc23be2..aa133e87a4 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -283,6 +283,12 @@ class SubscriptionBase : public std::enable_shared_from_this std::vector get_network_flow_endpoints() const; + RCLCPP_PUBLIC + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const; + protected: template void diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index c78b0a885f..54b141181b 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -200,6 +200,13 @@ class Waitable bool exchange_in_use_by_wait_set_state(bool in_use_state); + RCLCPP_PUBLIC + virtual + void + set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const; + private: std::atomic in_use_by_wait_set_{false}; }; // class Waitable diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index 6a1d3934d4..c74dd59c8d 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -198,3 +198,18 @@ ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } + +void +ClientBase::set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const +{ + rcl_ret_t ret = rcl_client_set_listener_callback( + client_handle_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set listener callback to client"); + } +} diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index daf1d3e01e..1335e38735 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -67,4 +67,20 @@ QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set) return wait_set->events[wait_set_event_index_] == &event_handle_; } +void +QOSEventHandlerBase::set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const +{ + rcl_ret_t ret = rcl_event_set_listener_callback( + &event_handle_, + callback, + user_data, + true /* Use previous events */); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set listener callback to QOSEventHandlerBase"); + } +} + } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index 827062bdd3..e279668bdf 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -84,3 +84,18 @@ ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } + +void +ServiceBase::set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const +{ + rcl_ret_t ret = rcl_service_set_listener_callback( + service_handle_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set listener callback to service"); + } +} diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 472034f362..cf593cb0bb 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -290,7 +290,8 @@ SubscriptionBase::exchange_in_use_by_wait_set_state( throw std::runtime_error("given pointer_to_subscription_part does not match any part"); } -std::vector SubscriptionBase::get_network_flow_endpoints() const +std::vector +SubscriptionBase::get_network_flow_endpoints() const { rcutils_allocator_t allocator = rcutils_get_default_allocator(); rcl_network_flow_endpoint_array_t network_flow_endpoint_array = @@ -326,3 +327,18 @@ std::vector SubscriptionBase::get_network_flow_endp return network_flow_endpoint_vector; } + +void +SubscriptionBase::set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const +{ + rcl_ret_t ret = rcl_subscription_set_listener_callback( + subscription_handle_.get(), + callback, + user_data); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set listener callback to subscription"); + } +} diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index b13123b65b..dd184bac41 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -34,3 +34,19 @@ SubscriptionIntraProcessBase::get_actual_qos() const { return qos_profile_; } + +void +SubscriptionIntraProcessBase::set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const +{ + rcl_ret_t ret = rcl_guard_condition_set_listener_callback( + &gc_, + callback, + user_data, + true /*Use previous events*/); + + if (RCL_RET_OK != ret) { + throw std::runtime_error("Couldn't set guard condition listener callback"); + } +} diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index b76c7215e0..09164f2e35 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "rclcpp/waitable.hpp" using rclcpp::Waitable; @@ -57,3 +59,15 @@ Waitable::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } + +void +Waitable::set_listener_callback( + rmw_listener_callback_t callback, + const void * user_data) const +{ + (void)callback; + (void)user_data; + + throw std::runtime_error( + "Custom waitables should override set_listener_callback() if they want to use RMW listeners"); +} From 9d9d75348d9e774f984d4d7028e01493e26f02b9 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Tue, 16 Mar 2021 11:20:28 +0000 Subject: [PATCH 02/31] split long log into two lines Signed-off-by: Alberto Soragna --- rclcpp/src/rclcpp/waitable.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 09164f2e35..ed2398bc0d 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -69,5 +69,6 @@ Waitable::set_listener_callback( (void)user_data; throw std::runtime_error( - "Custom waitables should override set_listener_callback() if they want to use RMW listeners"); + "Custom waitables should override set_listener_callback() " + "if they want to use RMW listeners"); } From e4ae5f5cfd157269740a4cdbbefb8e8fe56de00a Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 24 Mar 2021 16:53:21 -0300 Subject: [PATCH 03/31] Remove use_previous_event Signed-off-by: Mauro Passerino --- rclcpp/src/rclcpp/qos_event.cpp | 3 +-- rclcpp/src/rclcpp/subscription_intra_process_base.cpp | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 1335e38735..8783cfdc00 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -75,8 +75,7 @@ QOSEventHandlerBase::set_listener_callback( rcl_ret_t ret = rcl_event_set_listener_callback( &event_handle_, callback, - user_data, - true /* Use previous events */); + user_data); if (RCL_RET_OK != ret) { throw std::runtime_error("Couldn't set listener callback to QOSEventHandlerBase"); diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index dd184bac41..1058107a3d 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -43,8 +43,7 @@ SubscriptionIntraProcessBase::set_listener_callback( rcl_ret_t ret = rcl_guard_condition_set_listener_callback( &gc_, callback, - user_data, - true /*Use previous events*/); + user_data); if (RCL_RET_OK != ret) { throw std::runtime_error("Couldn't set guard condition listener callback"); From c1d4a84463bb5f1bdb30f2c61dee6d3c8f8adf79 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Thu, 1 Apr 2021 08:24:57 -0300 Subject: [PATCH 04/31] Do not set gc listener callback Signed-off-by: Mauro Passerino --- rclcpp/src/rclcpp/subscription_intra_process_base.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 1058107a3d..c9a0dc1d3e 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -40,12 +40,6 @@ SubscriptionIntraProcessBase::set_listener_callback( rmw_listener_callback_t callback, const void * user_data) const { - rcl_ret_t ret = rcl_guard_condition_set_listener_callback( - &gc_, - callback, - user_data); - - if (RCL_RET_OK != ret) { - throw std::runtime_error("Couldn't set guard condition listener callback"); - } + (void)callback; + (void)user_data; } From 28dbd07aefb641e1999720e4e9aea7e60f8c66de Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 1 Apr 2021 20:58:12 -0700 Subject: [PATCH 05/31] significant refactor to make callbacks type safe Signed-off-by: William Woodall --- rclcpp/include/rclcpp/client.hpp | 79 +++++++++++++++-- .../rclcpp/detail/cpp_callback_trampoline.hpp | 67 ++++++++++++++ .../subscription_intra_process_base.hpp | 20 +++-- rclcpp/include/rclcpp/qos_event.hpp | 77 ++++++++++++++-- rclcpp/include/rclcpp/service.hpp | 87 +++++++++++++++++-- rclcpp/include/rclcpp/subscription_base.hpp | 74 +++++++++++++++- rclcpp/include/rclcpp/waitable.hpp | 7 -- rclcpp/src/rclcpp/client.cpp | 12 +-- rclcpp/src/rclcpp/context.cpp | 2 - rclcpp/src/rclcpp/qos_event.cpp | 11 +-- rclcpp/src/rclcpp/service.cpp | 12 +-- rclcpp/src/rclcpp/subscription_base.cpp | 12 +-- .../subscription_intra_process_base.cpp | 9 -- rclcpp/src/rclcpp/waitable.cpp | 13 --- 14 files changed, 395 insertions(+), 87 deletions(-) create mode 100644 rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 6d50e88cfd..db83e0d853 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -31,18 +31,19 @@ #include "rcl/error_handling.h" #include "rcl/wait.h" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/function_traits.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" -#include "rcutils/logging_macros.h" - #include "rmw/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" #include "rmw/rmw.h" namespace rclcpp @@ -215,11 +216,68 @@ class ClientBase bool exchange_in_use_by_wait_set_state(bool in_use_state); - RCLCPP_PUBLIC + /// Set a callback to be called when each new response is received. + /** + * The callback receives a size_t which is the number of responses received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if responses were received before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * 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 client + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_client_set_on_new_response_callback + * \sa rcl_client_set_on_new_response_callback + * + * \param[in] callback functor to be called when a new response is received + */ void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + set_on_new_response_callback(std::function callback) + { + auto new_callback = + [callback, this](size_t number_of_responses) { + try { + callback(number_of_responses); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::ClientBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on new response' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::ClientBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on new response' callback"); + } + }; + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_new_response_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_new_response_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_new_response_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&on_new_response_callback_)); + } protected: RCLCPP_DISABLE_COPY(ClientBase) @@ -236,13 +294,20 @@ class ClientBase const rcl_node_t * get_rcl_node_handle() const; + RCLCPP_PUBLIC + void + set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data); + rclcpp::node_interfaces::NodeGraphInterface::WeakPtr node_graph_; std::shared_ptr node_handle_; std::shared_ptr context_; + rclcpp::Logger node_logger_; std::shared_ptr client_handle_; std::atomic in_use_by_wait_set_{false}; + + std::function on_new_response_callback_; }; template diff --git a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp new file mode 100644 index 0000000000..7ea991c0de --- /dev/null +++ b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp @@ -0,0 +1,67 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_ +#define RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_ + +#include + +namespace rclcpp +{ + +namespace detail +{ + +/// Trampoline pattern for wrapping std::function into C-style callbacks. +/** + * A common pattern in C is for a function to take a function pointer and a + * void pointer for "user data" which is passed to the function pointer when it + * is called from within C. + * + * It works by using the user data pointer to store a pointer to a + * std::function instance. + * So when called from C, this function will cast the user data to the right + * std::function type and call it. + * + * This should allow you to use free functions, lambdas with and without + * captures, and various kinds of std::bind instances. + * + * The interior of this function is likely to be executed within a C runtime, + * so no exceptins should be thrown at this point, and doing so results in + * undefined behavior. + * + * \tparam UserDataT Deduced type based on what is passed for user data, + * usually this type is either `void *` or `const void *`. + * \tparam Args the arguments being passed to the callback + * \tparam ReturnT the return type of this function and the callback, default void + * \param user_data the function pointer, possibly type erased + * \returns whatever the callback returns, if anything + */ +template< + typename UserDataT, + typename ... Args, + typename ReturnT = void +> +ReturnT +cpp_callback_trampoline(UserDataT user_data, Args ... args) noexcept +{ + auto & actual_callback = *reinterpret_cast *>(user_data); + return actual_callback(args ...); +} + +} // namespace detail + +} // namespace rclcpp + +#endif // RCLCPP__DETAIL__CPP_CALLBACK_TRAMPOLINE_HPP_ diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index a0bdddaf0e..c683da4fc5 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -55,10 +55,20 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable get_number_of_ready_guard_conditions() override {return 1;} RCLCPP_PUBLIC - void + bool add_to_wait_set(rcl_wait_set_t * wait_set) override; - virtual bool + bool + is_ready(rcl_wait_set_t * wait_set) override = 0; + + std::shared_ptr + take_data() override = 0; + + void + execute(std::shared_ptr & data) override = 0; + + virtual + bool use_take_shared_method() const = 0; RCLCPP_PUBLIC @@ -69,12 +79,6 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable QoS get_actual_qos() const; - RCLCPP_PUBLIC - void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const override; - protected: std::recursive_mutex reentrant_mutex_; rclcpp::GuardCondition gc_; diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 6263323e6d..c5e25d2c1f 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -21,12 +21,15 @@ #include #include "rcl/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" #include "rmw/incompatible_qos_events_statuses.h" #include "rcutils/logging_macros.h" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/function_traits.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/waitable.hpp" namespace rclcpp @@ -102,15 +105,78 @@ class QOSEventHandlerBase : public Waitable bool is_ready(rcl_wait_set_t * wait_set) override; - RCLCPP_PUBLIC + /// Set a callback to be called when each new event instance occurs. + /** + * The callback receives a size_t which is the number of events that occurred + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if events occurred before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * 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 qos event + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_event_set_callback + * \sa rcl_event_set_callback + * + * \param[in] callback functor to be called when a new event occurs + */ void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const override; + set_on_new_event_callback(std::function callback) + { + auto new_callback = + [callback, this](size_t number_of_events) { + try { + callback(number_of_events); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + // TODO(wjwwood): get this class access to the node logger it is associated with + rclcpp::get_logger("rclcpp"), + "rclcpp::QOSEventHandlerBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on new event' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::QOSEventHandlerBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on new event' callback"); + } + }; + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_new_event_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_new_event_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_new_event_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&on_new_event_callback_)); + } protected: + RCLCPP_PUBLIC + void + set_on_new_event_callback(rcl_event_callback_t callback, const void * user_data); + rcl_event_t event_handle_; size_t wait_set_event_index_; + std::function on_new_event_callback_; }; template @@ -123,9 +189,8 @@ class QOSEventHandler : public QOSEventHandlerBase InitFuncT init_func, ParentHandleT parent_handle, EventTypeEnum event_type) - : event_callback_(callback) + : parent_handle_(parent_handle), event_callback_(callback) { - parent_handle_ = parent_handle; event_handle_ = rcl_get_zero_initialized_event(); rcl_ret_t ret = init_func(&event_handle_, parent_handle.get(), event_type); if (ret != RCL_RET_OK) { diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index b427cc98e4..87c17f147a 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -25,16 +25,20 @@ #include "rcl/error_handling.h" #include "rcl/service.h" +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" +#include "rmw/rmw.h" + +#include "tracetools/tracetools.h" + #include "rclcpp/any_service_callback.hpp" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/exceptions.hpp" +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/type_support_decl.hpp" -#include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/visibility_control.hpp" -#include "rclcpp/logging.hpp" -#include "rmw/error_handling.h" -#include "rmw/rmw.h" -#include "tracetools/tracetools.h" namespace rclcpp { @@ -121,11 +125,68 @@ class ServiceBase bool exchange_in_use_by_wait_set_state(bool in_use_state); - RCLCPP_PUBLIC + /// Set a callback to be called when each new request is received. + /** + * The callback receives a size_t which is the number of requests received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if requests were received before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * 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 service + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_service_set_on_new_request_callback + * \sa rcl_service_set_on_new_request_callback + * + * \param[in] callback functor to be called when a new request is received + */ void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + set_on_new_request_callback(std::function callback) + { + auto new_callback = + [callback, this](size_t number_of_requests) { + try { + callback(number_of_requests); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::ServiceBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on new request' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::ServiceBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on new request' callback"); + } + }; + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_new_request_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_new_request_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_new_request_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&on_new_request_callback_)); + } protected: RCLCPP_DISABLE_COPY(ServiceBase) @@ -138,12 +199,20 @@ class ServiceBase const rcl_node_t * get_rcl_node_handle() const; + RCLCPP_PUBLIC + void + set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data); + std::shared_ptr node_handle_; std::shared_ptr service_handle_; bool owns_rcl_handle_ = true; + rclcpp::Logger node_logger_; + std::atomic in_use_by_wait_set_{false}; + + std::function on_new_request_callback_; }; template diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index aa133e87a4..abc7c751d8 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -24,9 +24,11 @@ #include "rcl/subscription.h" +#include "rmw/impl/cpp/demangle.hpp" #include "rmw/rmw.h" #include "rclcpp/any_subscription_callback.hpp" +#include "rclcpp/detail/cpp_callback_trampoline.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/experimental/subscription_intra_process_base.hpp" #include "rclcpp/macros.hpp" @@ -283,11 +285,68 @@ class SubscriptionBase : public std::enable_shared_from_this std::vector get_network_flow_endpoints() const; - RCLCPP_PUBLIC + /// Set a callback to be called when each new message is received. + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * 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 subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rmw_subscription_set_on_new_message_callback + * \sa rcl_subscription_set_on_new_message_callback + * + * \param[in] callback functor to be called when a new message is received + */ void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; + set_on_new_message_callback(std::function callback) + { + auto new_callback = + [callback, this](size_t number_of_messages) { + try { + callback(number_of_messages); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::SubscriptionBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on new message' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + node_logger_, + "rclcpp::SubscriptionBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on new message' callback"); + } + }; + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_new_message_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + // Store the std::function to keep it in scope, also overwrites the existing one. + on_new_message_callback_ = new_callback; + + // Set it again, now using the permanent storage. + set_on_new_message_callback( + rclcpp::detail::cpp_callback_trampoline, + static_cast(&on_new_message_callback_)); + } protected: template @@ -313,11 +372,16 @@ class SubscriptionBase : public std::enable_shared_from_this bool matches_any_intra_process_publishers(const rmw_gid_t * sender_gid) const; + RCLCPP_PUBLIC + void + set_on_new_message_callback(rcl_event_callback_t callback, const void * user_data); + rclcpp::node_interfaces::NodeBaseInterface * const node_base_; std::shared_ptr node_handle_; std::shared_ptr subscription_handle_; std::shared_ptr intra_process_subscription_handle_; + rclcpp::Logger node_logger_; std::vector> event_handlers_; @@ -335,6 +399,8 @@ class SubscriptionBase : public std::enable_shared_from_this std::atomic intra_process_subscription_waitable_in_use_by_wait_set_{false}; std::unordered_map> qos_events_in_use_by_wait_set_; + + std::function on_new_message_callback_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 54b141181b..c78b0a885f 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -200,13 +200,6 @@ class Waitable bool exchange_in_use_by_wait_set_state(bool in_use_state); - RCLCPP_PUBLIC - virtual - void - set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const; - private: std::atomic in_use_by_wait_set_{false}; }; // class Waitable diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index c74dd59c8d..e340a7d019 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -38,7 +38,8 @@ ClientBase::ClientBase( rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph) : node_graph_(node_graph), node_handle_(node_base->get_shared_rcl_node_handle()), - context_(node_base->get_context()) + context_(node_base->get_context()), + node_logger_(rclcpp::get_node_logger(node_handle_.get())) { std::weak_ptr weak_node_handle(node_handle_); rcl_client_t * new_rcl_client = new rcl_client_t; @@ -200,16 +201,15 @@ ClientBase::exchange_in_use_by_wait_set_state(bool in_use_state) } void -ClientBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const +ClientBase::set_on_new_response_callback(rcl_event_callback_t callback, const void * user_data) { - rcl_ret_t ret = rcl_client_set_listener_callback( + rcl_ret_t ret = rcl_client_set_on_new_response_callback( client_handle_.get(), callback, user_data); if (RCL_RET_OK != ret) { - throw std::runtime_error("Couldn't set listener callback to client"); + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on new response callback for client"); } } diff --git a/rclcpp/src/rclcpp/context.cpp b/rclcpp/src/rclcpp/context.cpp index d3d123c065..bea4eeb583 100644 --- a/rclcpp/src/rclcpp/context.cpp +++ b/rclcpp/src/rclcpp/context.cpp @@ -32,8 +32,6 @@ #include "rcutils/error_handling.h" #include "rcutils/macros.h" -#include "rmw/impl/cpp/demangle.hpp" - #include "./logging_mutex.hpp" using rclcpp::Context; diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index 8783cfdc00..aa35199501 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -68,17 +68,18 @@ QOSEventHandlerBase::is_ready(rcl_wait_set_t * wait_set) } void -QOSEventHandlerBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const +QOSEventHandlerBase::set_on_new_event_callback( + rcl_event_callback_t callback, + const void * user_data) { - rcl_ret_t ret = rcl_event_set_listener_callback( + rcl_ret_t ret = rcl_event_set_callback( &event_handle_, callback, user_data); if (RCL_RET_OK != ret) { - throw std::runtime_error("Couldn't set listener callback to QOSEventHandlerBase"); + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on new message callback for subscription"); } } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index e279668bdf..bfd9aba0c0 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -28,7 +28,8 @@ using rclcpp::ServiceBase; ServiceBase::ServiceBase(std::shared_ptr node_handle) -: node_handle_(node_handle) +: node_handle_(node_handle), + node_logger_(rclcpp::get_node_logger(node_handle_.get())) {} ServiceBase::~ServiceBase() @@ -86,16 +87,15 @@ ServiceBase::exchange_in_use_by_wait_set_state(bool in_use_state) } void -ServiceBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const +ServiceBase::set_on_new_request_callback(rcl_event_callback_t callback, const void * user_data) { - rcl_ret_t ret = rcl_service_set_listener_callback( + rcl_ret_t ret = rcl_service_set_on_new_request_callback( service_handle_.get(), callback, user_data); if (RCL_RET_OK != ret) { - throw std::runtime_error("Couldn't set listener callback to service"); + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on new request callback for service"); } } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index cf593cb0bb..8dd5c36086 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -39,6 +39,7 @@ SubscriptionBase::SubscriptionBase( bool is_serialized) : node_base_(node_base), node_handle_(node_base_->get_shared_rcl_node_handle()), + node_logger_(rclcpp::get_node_logger(node_handle_.get())), use_intra_process_(false), intra_process_subscription_id_(0), type_support_(type_support_handle), @@ -329,16 +330,17 @@ SubscriptionBase::get_network_flow_endpoints() const } void -SubscriptionBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const +SubscriptionBase::set_on_new_message_callback( + rcl_event_callback_t callback, + const void * user_data) { - rcl_ret_t ret = rcl_subscription_set_listener_callback( + rcl_ret_t ret = rcl_subscription_set_on_new_message_callback( subscription_handle_.get(), callback, user_data); if (RCL_RET_OK != ret) { - throw std::runtime_error("Couldn't set listener callback to subscription"); + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on new message callback for subscription"); } } diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index c9a0dc1d3e..b13123b65b 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -34,12 +34,3 @@ SubscriptionIntraProcessBase::get_actual_qos() const { return qos_profile_; } - -void -SubscriptionIntraProcessBase::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const -{ - (void)callback; - (void)user_data; -} diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index ed2398bc0d..ef3a50a8d9 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -59,16 +59,3 @@ Waitable::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } - -void -Waitable::set_listener_callback( - rmw_listener_callback_t callback, - const void * user_data) const -{ - (void)callback; - (void)user_data; - - throw std::runtime_error( - "Custom waitables should override set_listener_callback() " - "if they want to use RMW listeners"); -} From 780d05344a33f621a245d0d6c5dea09f87ca30d9 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Mon, 19 Apr 2021 12:19:19 +0100 Subject: [PATCH 06/31] Add on_ready callback to waitables, add clear_* functions, add unit-tests Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/client.hpp | 21 +++- .../subscription_intra_process.hpp | 41 ++++++++ .../subscription_intra_process_base.hpp | 97 ++++++++++++++++++- rclcpp/include/rclcpp/qos_event.hpp | 54 +++++++++-- rclcpp/include/rclcpp/service.hpp | 22 ++++- rclcpp/include/rclcpp/subscription_base.hpp | 18 +++- rclcpp/include/rclcpp/waitable.hpp | 40 ++++++++ rclcpp/src/rclcpp/waitable.cpp | 18 ++++ rclcpp/test/rclcpp/test_client.cpp | 91 +++++++++++++++++ rclcpp/test/rclcpp/test_service.cpp | 71 ++++++++++++++ rclcpp/test/rclcpp/test_subscription.cpp | 70 +++++++++++++ 11 files changed, 529 insertions(+), 14 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index db83e0d853..3f88974d34 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -230,19 +230,29 @@ class ClientBase * * Calling it again will clear any previously set callback. * + * An exception will be thrown if the callback is not callable. + * * This function is thread-safe. * * If you want more information available in the callback, like the client * or other information, you may use a lambda with captures or std::bind. * + * \sa rclcpp::ClientBase::clear_on_new_response_callback * \sa rmw_client_set_on_new_response_callback * \sa rcl_client_set_on_new_response_callback * * \param[in] callback functor to be called when a new response is received */ + RCLCPP_PUBLIC void set_on_new_response_callback(std::function callback) { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_response_callback " + "is not callable."); + } + auto new_callback = [callback, this](size_t number_of_responses) { try { @@ -279,6 +289,15 @@ class ClientBase static_cast(&on_new_response_callback_)); } + /// Unset the callback registered for new responses, if any. + RCLCPP_PUBLIC + void + clear_on_new_response_callback() + { + set_on_new_response_callback(nullptr, nullptr); + on_new_response_callback_ = nullptr; + } + protected: RCLCPP_DISABLE_COPY(ClientBase) @@ -307,7 +326,7 @@ class ClientBase std::atomic in_use_by_wait_set_{false}; - std::function on_new_response_callback_; + std::function on_new_response_callback_{nullptr}; }; template diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 54857562d0..63399d8ea5 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -127,6 +127,47 @@ class SubscriptionIntraProcess execute_impl(data); } + void + provide_intra_process_message(ConstMessageSharedPtr message) + { + buffer_->add_shared(std::move(message)); + invoke_callback(); + trigger_guard_condition(); + } + + void + provide_intra_process_message(MessageUniquePtr message) + { + buffer_->add_unique(std::move(message)); + invoke_callback(); + trigger_guard_condition(); + } + + bool + use_take_shared_method() const + { + return buffer_->use_take_shared_method(); + } + +private: + void + invoke_callback() + { + std::lock_guard lock(reentrant_mutex_); + if (on_new_message_callback_) { + on_new_message_callback_(1); + } else { + unread_count_++; + } + } + + void + trigger_guard_condition() + { + rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); + (void)ret; + } + protected: template typename std::enable_if::value, void>::type diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index c683da4fc5..7826adb1a1 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -15,17 +15,17 @@ #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ -#include - -#include +#include #include #include #include #include #include "rcl/error_handling.h" +#include "rmw/impl/cpp/demangle.hpp" #include "rclcpp/guard_condition.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/qos.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/waitable.hpp" @@ -40,6 +40,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable public: RCLCPP_SMART_PTR_ALIASES_ONLY(SubscriptionIntraProcessBase) + enum class EntityType + { + Subscription, + }; + RCLCPP_PUBLIC SubscriptionIntraProcessBase( rclcpp::Context::SharedPtr context, @@ -79,8 +84,94 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable QoS get_actual_qos() const; + /// Set a callback to be called when each new event instance occurs. + /** + * The callback receives a size_t which is the number of responses received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if responses were received before any + * callback was set. + * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bounded to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the client + * or other information, you may use a lambda with captures or std::bind. + * + * Note: this function must be overridden with a proper implementation + * by the custom classes who inherit from rclcpp::Waitable if they want to use it. + * + * \sa rclcpp::SubscriptionIntraProcessBase::clear_on_ready_callback + * + * \param[in] callback functor to be called when a new message is received. + */ + RCLCPP_PUBLIC + void + set_on_ready_callback(std::function callback) override + { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(EntityType::Subscription)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + // TODO(wjwwood): get this class access to the node logger it is associated with + rclcpp::get_logger("rclcpp"), + "rclcpp::SubscriptionIntraProcessBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("rclcpp"), + "rclcpp::SubscriptionIntraProcessBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + std::lock_guard lock(reentrant_mutex_); + on_new_message_callback_ = new_callback; + + if (unread_count_ > 0) { + // Use qos profile depth as upper bound for unread_count_ + on_new_message_callback_(std::min(unread_count_, qos_profile_.depth)); + unread_count_ = 0; + } + } + + /// Unset the callback registered for new messages, if any. + RCLCPP_PUBLIC + void + clear_on_ready_callback() override + { + std::lock_guard lock(reentrant_mutex_); + on_new_message_callback_ = nullptr; + } + protected: std::recursive_mutex reentrant_mutex_; + std::function on_new_message_callback_ {nullptr}; + size_t unread_count_{0}; rclcpp::GuardCondition gc_; virtual void diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index c5e25d2c1f..4c08040090 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -87,6 +87,11 @@ class UnsupportedEventTypeException : public exceptions::RCLErrorBase, public st class QOSEventHandlerBase : public Waitable { public: + enum class EntityType + { + Event, + }; + RCLCPP_PUBLIC virtual ~QOSEventHandlerBase(); @@ -107,11 +112,21 @@ class QOSEventHandlerBase : public Waitable /// Set a callback to be called when each new event instance occurs. /** - * The callback receives a size_t which is the number of events that occurred + * The callback receives a size_t which is the number of responses received * since the last time this callback was called. - * Normally this is 1, but can be > 1 if events occurred before any + * Normally this is 1, but can be > 1 if responses were received before any * callback was set. * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bounded to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * * Since this callback is called from the middleware, you should aim to make * it fast and not blocking. * If you need to do a lot of work or wait for some other event, you should @@ -119,37 +134,51 @@ class QOSEventHandlerBase : public Waitable * * Calling it again will clear any previously set callback. * + * An exception will be thrown if the callback is not callable. + * * This function is thread-safe. * - * If you want more information available in the callback, like the qos event + * If you want more information available in the callback, like the client * or other information, you may use a lambda with captures or std::bind. * + * Note: this function must be overridden with a proper implementation + * by the custom classes who inherit from rclcpp::Waitable if they want to use it. + * + * \sa rclcpp::QOSEventHandlerBase::clear_on_ready_callback * \sa rmw_event_set_callback * \sa rcl_event_set_callback * * \param[in] callback functor to be called when a new event occurs */ + RCLCPP_PUBLIC void - set_on_new_event_callback(std::function callback) + set_on_ready_callback(std::function callback) override { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + // Note: we bind the int identifier argument to this waitable's entity types auto new_callback = [callback, this](size_t number_of_events) { try { - callback(number_of_events); + callback(number_of_events, static_cast(EntityType::Event)); } catch (const std::exception & exception) { RCLCPP_ERROR_STREAM( // TODO(wjwwood): get this class access to the node logger it is associated with rclcpp::get_logger("rclcpp"), "rclcpp::QOSEventHandlerBase@" << this << " caught " << rmw::impl::cpp::demangle(exception) << - " exception in user-provided callback for the 'on new event' callback: " << + " exception in user-provided callback for the 'on ready' callback: " << exception.what()); } catch (...) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("rclcpp"), "rclcpp::QOSEventHandlerBase@" << this << " caught unhandled exception in user-provided callback " << - "for the 'on new event' callback"); + "for the 'on ready' callback"); } }; @@ -169,6 +198,15 @@ class QOSEventHandlerBase : public Waitable static_cast(&on_new_event_callback_)); } + /// Unset the callback registered for new events, if any. + RCLCPP_PUBLIC + void + clear_on_ready_callback() override + { + set_on_new_event_callback(nullptr, nullptr); + on_new_event_callback_ = nullptr; + } + protected: RCLCPP_PUBLIC void @@ -176,7 +214,7 @@ class QOSEventHandlerBase : public Waitable rcl_event_t event_handle_; size_t wait_set_event_index_; - std::function on_new_event_callback_; + std::function on_new_event_callback_{nullptr}; }; template diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 87c17f147a..c89c2b9518 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -139,19 +139,30 @@ class ServiceBase * * Calling it again will clear any previously set callback. * + * + * An exception will be thrown if the callback is not callable. + * * This function is thread-safe. * * If you want more information available in the callback, like the service * or other information, you may use a lambda with captures or std::bind. * + * \sa rclcpp::ServiceBase::clear_on_new_request_callback * \sa rmw_service_set_on_new_request_callback * \sa rcl_service_set_on_new_request_callback * * \param[in] callback functor to be called when a new request is received */ + RCLCPP_PUBLIC void set_on_new_request_callback(std::function callback) { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_request_callback " + "is not callable."); + } + auto new_callback = [callback, this](size_t number_of_requests) { try { @@ -188,6 +199,15 @@ class ServiceBase static_cast(&on_new_request_callback_)); } + /// Unset the callback registered for new requests, if any. + RCLCPP_PUBLIC + void + clear_on_new_request_callback() + { + set_on_new_request_callback(nullptr, nullptr); + on_new_request_callback_ = nullptr; + } + protected: RCLCPP_DISABLE_COPY(ServiceBase) @@ -212,7 +232,7 @@ class ServiceBase std::atomic in_use_by_wait_set_{false}; - std::function on_new_request_callback_; + std::function on_new_request_callback_{nullptr}; }; template diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index abc7c751d8..3b67829d13 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -309,9 +309,16 @@ class SubscriptionBase : public std::enable_shared_from_this * * \param[in] callback functor to be called when a new message is received */ + RCLCPP_PUBLIC void set_on_new_message_callback(std::function callback) { + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_message_callback " + "is not callable."); + } + auto new_callback = [callback, this](size_t number_of_messages) { try { @@ -348,6 +355,15 @@ class SubscriptionBase : public std::enable_shared_from_this static_cast(&on_new_message_callback_)); } + /// Unset the callback registered for new messages, if any. + RCLCPP_PUBLIC + void + clear_on_new_message_callback() + { + set_on_new_message_callback(nullptr, nullptr); + on_new_message_callback_ = nullptr; + } + protected: template void @@ -400,7 +416,7 @@ class SubscriptionBase : public std::enable_shared_from_this std::unordered_map> qos_events_in_use_by_wait_set_; - std::function on_new_message_callback_; + std::function on_new_message_callback_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index c78b0a885f..412f4d4903 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -16,6 +16,7 @@ #define RCLCPP__WAITABLE_HPP_ #include +#include #include #include "rclcpp/macros.hpp" @@ -200,6 +201,45 @@ class Waitable bool exchange_in_use_by_wait_set_state(bool in_use_state); + /// Set a callback to be called whenever the waitable becomes ready. + /** + * The callback receives a size_t which is the number of responses received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if responses were received before any + * callback was set. + * + * The callback also receives an int identifier argument. + * This is needed because a Waitable may be composed of several distinct entities, + * such as subscriptions, services, etc. + * The application should provide a generic callback function that will be then + * forwarded by the waitable to all of its entities. + * Before forwarding, a different value for the identifier argument will be + * bounded to the function. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Note: this function must be overridden with a proper implementation + * by the custom classes who inherit from rclcpp::Waitable if they want to use it. + * + * \sa rclcpp::Waitable::clear_on_ready_callback + * + * \param[in] callback functor to be called when the waitable becomes ready + */ + RCLCPP_PUBLIC + virtual + void + set_on_ready_callback(std::function callback); + + /// Unset any callback registered via set_on_ready_callback. + /** + * Note: this function must be overridden with a proper implementation + * by the custom classes who inherit from rclcpp::Waitable if they want to use it. + */ + RCLCPP_PUBLIC + virtual + void + clear_on_ready_callback(); + private: std::atomic in_use_by_wait_set_{false}; }; // class Waitable diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index ef3a50a8d9..a42968dc95 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -59,3 +59,21 @@ Waitable::exchange_in_use_by_wait_set_state(bool in_use_state) { return in_use_by_wait_set_.exchange(in_use_state); } + +void +Waitable::set_on_ready_callback(std::function callback) +{ + (void)callback; + + throw std::runtime_error( + "Custom waitables should override set_on_ready_callback " + "if they want to use it."); +} + +void +Waitable::clear_on_ready_callback() +{ + throw std::runtime_error( + "Custom waitables should override clear_on_ready_callback " + "if they want to use it."); +} diff --git a/rclcpp/test/rclcpp/test_client.cpp b/rclcpp/test/rclcpp/test_client.cpp index 5e2adb5e7d..5e18fd8b4a 100644 --- a/rclcpp/test/rclcpp/test_client.cpp +++ b/rclcpp/test/rclcpp/test_client.cpp @@ -27,6 +27,8 @@ #include "test_msgs/srv/empty.hpp" +using namespace std::chrono_literals; + class TestClient : public ::testing::Test { protected: @@ -340,3 +342,92 @@ TEST_F(TestClientWithServer, take_response) { rclcpp::exceptions::RCLError); } } + +/* + Testing on_new_response callbacks. + */ +TEST_F(TestClient, on_new_response_callback) { + auto client_node = std::make_shared("client_node", "ns"); + auto server_node = std::make_shared("server_node", "ns"); + + auto client = client_node->create_client("test_service"); + std::atomic server_requests_count {0}; + auto server_callback = [&server_requests_count]( + const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {server_requests_count++;}; + auto server = server_node->create_service("test_service", server_callback); + auto request = std::make_shared(); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + client->set_on_new_response_callback(increase_c1_cb); + + client->async_send_request(request); + auto start = std::chrono::steady_clock::now(); + while (server_requests_count == 0 && + (std::chrono::steady_clock::now() - start) < 10s) + { + rclcpp::spin_some(server_node); + } + + ASSERT_EQ(server_requests_count, 1u); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + client->set_on_new_response_callback(increase_c2_cb); + + client->async_send_request(request); + start = std::chrono::steady_clock::now(); + while (server_requests_count == 1 && + (std::chrono::steady_clock::now() - start) < 10s) + { + rclcpp::spin_some(server_node); + } + + ASSERT_EQ(server_requests_count, 2u); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + client->clear_on_new_response_callback(); + + client->async_send_request(request); + client->async_send_request(request); + client->async_send_request(request); + start = std::chrono::steady_clock::now(); + while (server_requests_count < 5 && + (std::chrono::steady_clock::now() - start) < 10s) + { + rclcpp::spin_some(server_node); + } + + ASSERT_EQ(server_requests_count, 5u); + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + client->set_on_new_response_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(client->set_on_new_response_callback(invalid_cb), std::invalid_argument); +} diff --git a/rclcpp/test/rclcpp/test_service.cpp b/rclcpp/test/rclcpp/test_service.cpp index 16ab31cf1b..110e913da4 100644 --- a/rclcpp/test/rclcpp/test_service.cpp +++ b/rclcpp/test/rclcpp/test_service.cpp @@ -27,6 +27,8 @@ #include "test_msgs/srv/empty.hpp" #include "test_msgs/srv/empty.h" +using namespace std::chrono_literals; + class TestService : public ::testing::Test { protected: @@ -235,3 +237,72 @@ TEST_F(TestService, send_response) { rclcpp::exceptions::RCLError); } } + +/* + Testing on_new_request callbacks. + */ +TEST_F(TestService, on_new_request_callback) { + auto server_callback = + [](const test_msgs::srv::Empty::Request::SharedPtr, + test_msgs::srv::Empty::Response::SharedPtr) {FAIL();}; + auto server = node->create_service("~/test_service", server_callback); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + server->set_on_new_request_callback(increase_c1_cb); + + auto client = node->create_client("~/test_service"); + { + auto request = std::make_shared(); + client->async_send_request(request); + } + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + server->set_on_new_request_callback(increase_c2_cb); + + { + auto request = std::make_shared(); + client->async_send_request(request); + } + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + server->clear_on_new_request_callback(); + + { + auto request = std::make_shared(); + client->async_send_request(request); + client->async_send_request(request); + client->async_send_request(request); + } + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + server->set_on_new_request_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(server->set_on_new_request_callback(invalid_cb), std::invalid_argument); +} diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 5812985272..a4b0f0f957 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -440,6 +440,76 @@ TEST_F(TestSubscription, handle_loaned_message) { EXPECT_NO_THROW(sub->handle_loaned_message(&msg, message_info)); } +/* + Testing on_new_message callbacks. + */ +TEST_F(TestSubscription, on_new_message_callback) { + initialize(); + using test_msgs::msg::Empty; + + auto do_nothing = [](std::shared_ptr) {FAIL();}; + auto sub = node->create_subscription("~/test_take", 1, do_nothing); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + sub->set_on_new_message_callback(increase_c1_cb); + + auto pub = node->create_publisher("~/test_take", 1); + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + sub->set_on_new_message_callback(increase_c2_cb); + + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + sub->clear_on_new_message_callback(); + + { + test_msgs::msg::Empty msg; + pub->publish(msg); + pub->publish(msg); + pub->publish(msg); + } + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + sub->set_on_new_message_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(sub->set_on_new_message_callback(invalid_cb), std::invalid_argument); +} + /* Testing subscription with intraprocess enabled and invalid QoS */ From e909eed250e9dcc5d49bec8100beb7a2d31bf749 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 11:04:16 +0100 Subject: [PATCH 07/31] add mutex to set and clear listener APIs Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/client.hpp | 7 +++++-- .../experimental/subscription_intra_process_base.hpp | 1 - rclcpp/include/rclcpp/qos_event.hpp | 5 +++++ rclcpp/include/rclcpp/service.hpp | 5 +++++ rclcpp/include/rclcpp/subscription_base.hpp | 7 +++++-- rclcpp/include/rclcpp/waitable.hpp | 2 -- 6 files changed, 20 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 3f88974d34..b4a90b4b7a 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include // NOLINT, cpplint doesn't think this is a cpp std header #include #include @@ -243,7 +244,6 @@ class ClientBase * * \param[in] callback functor to be called when a new response is received */ - RCLCPP_PUBLIC void set_on_new_response_callback(std::function callback) { @@ -273,6 +273,8 @@ class ClientBase } }; + std::lock_guard lock(reentrant_mutex_); + // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has // been replaced but the middleware hasn't been told about the new one yet. @@ -290,10 +292,10 @@ class ClientBase } /// Unset the callback registered for new responses, if any. - RCLCPP_PUBLIC void clear_on_new_response_callback() { + std::lock_guard lock(reentrant_mutex_); set_on_new_response_callback(nullptr, nullptr); on_new_response_callback_ = nullptr; } @@ -326,6 +328,7 @@ class ClientBase std::atomic in_use_by_wait_set_{false}; + std::recursive_mutex reentrant_mutex_; std::function on_new_response_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 7826adb1a1..341b5b6edb 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -117,7 +117,6 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable * * \param[in] callback functor to be called when a new message is received. */ - RCLCPP_PUBLIC void set_on_ready_callback(std::function callback) override { diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 4c08040090..93ba0e777a 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -182,6 +183,8 @@ class QOSEventHandlerBase : public Waitable } }; + std::lock_guard lock(reentrant_mutex_); + // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has // been replaced but the middleware hasn't been told about the new one yet. @@ -203,6 +206,7 @@ class QOSEventHandlerBase : public Waitable void clear_on_ready_callback() override { + std::lock_guard lock(reentrant_mutex_); set_on_new_event_callback(nullptr, nullptr); on_new_event_callback_ = nullptr; } @@ -214,6 +218,7 @@ class QOSEventHandlerBase : public Waitable rcl_event_t event_handle_; size_t wait_set_event_index_; + std::recursive_mutex reentrant_mutex_; std::function on_new_event_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index c89c2b9518..0eef8c75e8 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -183,6 +184,8 @@ class ServiceBase } }; + std::lock_guard lock(reentrant_mutex_); + // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has // been replaced but the middleware hasn't been told about the new one yet. @@ -204,6 +207,7 @@ class ServiceBase void clear_on_new_request_callback() { + std::lock_guard lock(reentrant_mutex_); set_on_new_request_callback(nullptr, nullptr); on_new_request_callback_ = nullptr; } @@ -232,6 +236,7 @@ class ServiceBase std::atomic in_use_by_wait_set_{false}; + std::recursive_mutex reentrant_mutex_; std::function on_new_request_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 3b67829d13..ab793913e7 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -309,7 +310,6 @@ class SubscriptionBase : public std::enable_shared_from_this * * \param[in] callback functor to be called when a new message is received */ - RCLCPP_PUBLIC void set_on_new_message_callback(std::function callback) { @@ -339,6 +339,8 @@ class SubscriptionBase : public std::enable_shared_from_this } }; + std::lock_guard lock(reentrant_mutex_); + // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has // been replaced but the middleware hasn't been told about the new one yet. @@ -356,10 +358,10 @@ class SubscriptionBase : public std::enable_shared_from_this } /// Unset the callback registered for new messages, if any. - RCLCPP_PUBLIC void clear_on_new_message_callback() { + std::lock_guard lock(reentrant_mutex_); set_on_new_message_callback(nullptr, nullptr); on_new_message_callback_ = nullptr; } @@ -416,6 +418,7 @@ class SubscriptionBase : public std::enable_shared_from_this std::unordered_map> qos_events_in_use_by_wait_set_; + std::recursive_mutex reentrant_mutex_; std::function on_new_message_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 412f4d4903..b040cad543 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -225,7 +225,6 @@ class Waitable * * \param[in] callback functor to be called when the waitable becomes ready */ - RCLCPP_PUBLIC virtual void set_on_ready_callback(std::function callback); @@ -235,7 +234,6 @@ class Waitable * Note: this function must be overridden with a proper implementation * by the custom classes who inherit from rclcpp::Waitable if they want to use it. */ - RCLCPP_PUBLIC virtual void clear_on_ready_callback(); From f4bb83aa08dd48b5adf2d8905a1beedb75abe32b Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 11:30:52 +0100 Subject: [PATCH 08/31] allow to set ipc sub callback from regular subscription Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/subscription_base.hpp | 50 ++++++++++++++ rclcpp/test/rclcpp/test_subscription.cpp | 72 ++++++++++++++++++++- 2 files changed, 121 insertions(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index ab793913e7..7937d54603 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -366,6 +366,55 @@ class SubscriptionBase : public std::enable_shared_from_this on_new_message_callback_ = nullptr; } + /// Set a callback to be called when each new intra-process message is received. + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received 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 subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rclcpp::SubscriptionIntraProcessBase::set_on_ready_callback + * + * \param[in] callback functor to be called when a new message is received + */ + void + set_on_new_intra_process_message_callback(std::function callback) + { + if (!use_intra_process_) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling set_on_new_intra_process_message_callback for subscription with IPC disabled"); + return; + } + + // The on_ready_callback signature has an extra `int` argument used to disambiguate between + // possible different entities within a generic waitable. + // We hide that detail to users of this method. + std::function new_callback = std::bind(callback, std::placeholders::_1); + subscription_intra_process_->set_on_ready_callback(new_callback); + } + + /// Unset the callback registered for new intra-process messages, if any. + void + clear_on_new_intra_process_message_callback() + { + if (!use_intra_process_) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling clear_on_new_intra_process_message_callback for subscription with IPC disabled"); + return; + } + + subscription_intra_process_->clear_on_ready_callback(); + } + protected: template void @@ -406,6 +455,7 @@ class SubscriptionBase : public std::enable_shared_from_this bool use_intra_process_; IntraProcessManagerWeakPtr weak_ipm_; uint64_t intra_process_subscription_id_; + std::shared_ptr subscription_intra_process_; private: RCLCPP_DISABLE_COPY(SubscriptionBase) diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index a4b0f0f957..3bfdabf51c 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -444,7 +444,7 @@ TEST_F(TestSubscription, handle_loaned_message) { Testing on_new_message callbacks. */ TEST_F(TestSubscription, on_new_message_callback) { - initialize(); + initialize(rclcpp::NodeOptions().use_intra_process_comms(false)); using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; @@ -510,6 +510,76 @@ TEST_F(TestSubscription, on_new_message_callback) { EXPECT_THROW(sub->set_on_new_message_callback(invalid_cb), std::invalid_argument); } +/* + Testing on_new_intra_process_message callbacks. + */ +TEST_F(TestSubscription, on_new_intra_process_message_callback) { + initialize(rclcpp::NodeOptions().use_intra_process_comms(true)); + using test_msgs::msg::Empty; + + auto do_nothing = [](std::shared_ptr) {FAIL();}; + auto sub = node->create_subscription("~/test_take", 1, do_nothing); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; + sub->set_on_new_intra_process_message_callback(increase_c1_cb); + + auto pub = node->create_publisher("~/test_take", 1); + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + sub->set_on_new_intra_process_message_callback(increase_c2_cb); + + { + test_msgs::msg::Empty msg; + pub->publish(msg); + } + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + sub->clear_on_new_intra_process_message_callback(); + + { + test_msgs::msg::Empty msg; + pub->publish(msg); + pub->publish(msg); + pub->publish(msg); + } + + std::atomic c3 {0}; + auto increase_c3_cb = [&c3](size_t count_msgs) {c3 += count_msgs;}; + sub->set_on_new_intra_process_message_callback(increase_c3_cb); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c3 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + EXPECT_EQ(c3.load(), 3u); + + std::function invalid_cb = nullptr; + EXPECT_THROW(sub->set_on_new_intra_process_message_callback(invalid_cb), std::invalid_argument); +} + /* Testing subscription with intraprocess enabled and invalid QoS */ From c653699d1cf2740f8e47341e1c3276107837dbc9 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 13:36:55 +0100 Subject: [PATCH 09/31] fix minor failures Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/subscription.hpp | 4 ++++ rclcpp/include/rclcpp/subscription_base.hpp | 10 ++++++++-- rclcpp/test/rclcpp/test_subscription.cpp | 4 ++-- 3 files changed, 14 insertions(+), 4 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 2d52388e3b..70d2676d3c 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -199,6 +199,10 @@ class Subscription : public SubscriptionBase // First create a SubscriptionIntraProcess which will be given to the intra-process manager. auto context = node_base->get_context(); + using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< + CallbackMessageT, + AllocatorT, + typename MessageUniquePtr::deleter_type>; subscription_intra_process_ = std::make_shared( callback, options_.get_allocator(), diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 7937d54603..807083c173 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -394,7 +394,13 @@ class SubscriptionBase : public std::enable_shared_from_this return; } - // The on_ready_callback signature has an extra `int` argument used to disambiguate between + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_intra_process_message_callback " + "is not callable."); + } + + // The on_ready_callback signature has an extra `int` argument used to disambiguate between // possible different entities within a generic waitable. // We hide that detail to users of this method. std::function new_callback = std::bind(callback, std::placeholders::_1); @@ -455,7 +461,7 @@ class SubscriptionBase : public std::enable_shared_from_this bool use_intra_process_; IntraProcessManagerWeakPtr weak_ipm_; uint64_t intra_process_subscription_id_; - std::shared_ptr subscription_intra_process_; + std::shared_ptr subscription_intra_process_; private: RCLCPP_DISABLE_COPY(SubscriptionBase) diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 3bfdabf51c..a10c5c4eab 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -448,7 +448,7 @@ TEST_F(TestSubscription, on_new_message_callback) { using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; - auto sub = node->create_subscription("~/test_take", 1, do_nothing); + auto sub = node->create_subscription("~/test_take", 10, do_nothing); std::atomic c1 {0}; auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; @@ -518,7 +518,7 @@ TEST_F(TestSubscription, on_new_intra_process_message_callback) { using test_msgs::msg::Empty; auto do_nothing = [](std::shared_ptr) {FAIL();}; - auto sub = node->create_subscription("~/test_take", 1, do_nothing); + auto sub = node->create_subscription("~/test_take", 10, do_nothing); std::atomic c1 {0}; auto increase_c1_cb = [&c1](size_t count_msgs) {c1 += count_msgs;}; From a2b0186c1210c02034b1607d1f44baf5e5251e47 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 14:48:14 +0100 Subject: [PATCH 10/31] fix typos and errors in comments Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/client.hpp | 1 - .../subscription_intra_process_base.hpp | 15 +++++---------- rclcpp/include/rclcpp/qos_event.hpp | 12 ++++-------- rclcpp/include/rclcpp/service.hpp | 3 --- rclcpp/include/rclcpp/waitable.hpp | 2 +- 5 files changed, 10 insertions(+), 23 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index b4a90b4b7a..8da52f1675 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -238,7 +238,6 @@ class ClientBase * If you want more information available in the callback, like the client * or other information, you may use a lambda with captures or std::bind. * - * \sa rclcpp::ClientBase::clear_on_new_response_callback * \sa rmw_client_set_on_new_response_callback * \sa rcl_client_set_on_new_response_callback * diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 341b5b6edb..a3dfba83af 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -84,11 +84,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable QoS get_actual_qos() const; - /// Set a callback to be called when each new event instance occurs. + /// Set a callback to be called when each new message arrives. /** - * The callback receives a size_t which is the number of responses received + * The callback receives a size_t which is the number of messages received * since the last time this callback was called. - * Normally this is 1, but can be > 1 if responses were received before any + * Normally this is 1, but can be > 1 if messages were received before any * callback was set. * * The callback also receives an int identifier argument. @@ -97,7 +97,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable * The application should provide a generic callback function that will be then * forwarded by the waitable to all of its entities. * Before forwarding, a different value for the identifier argument will be - * bounded to the function. + * bond to the function. * This implies that the provided callback can use the identifier to behave * differently depending on which entity triggered the waitable to become ready. * @@ -107,14 +107,9 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable * * This function is thread-safe. * - * If you want more information available in the callback, like the client + * If you want more information available in the callback, like the subscription * or other information, you may use a lambda with captures or std::bind. * - * Note: this function must be overridden with a proper implementation - * by the custom classes who inherit from rclcpp::Waitable if they want to use it. - * - * \sa rclcpp::SubscriptionIntraProcessBase::clear_on_ready_callback - * * \param[in] callback functor to be called when a new message is received. */ void diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 93ba0e777a..748662a73b 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -113,9 +113,9 @@ class QOSEventHandlerBase : public Waitable /// Set a callback to be called when each new event instance occurs. /** - * The callback receives a size_t which is the number of responses received + * The callback receives a size_t which is the number of events that occurred * since the last time this callback was called. - * Normally this is 1, but can be > 1 if responses were received before any + * Normally this is 1, but can be > 1 if events occurred before any * callback was set. * * The callback also receives an int identifier argument. @@ -124,7 +124,7 @@ class QOSEventHandlerBase : public Waitable * The application should provide a generic callback function that will be then * forwarded by the waitable to all of its entities. * Before forwarding, a different value for the identifier argument will be - * bounded to the function. + * bond to the function. * This implies that the provided callback can use the identifier to behave * differently depending on which entity triggered the waitable to become ready. * @@ -139,13 +139,9 @@ class QOSEventHandlerBase : public Waitable * * This function is thread-safe. * - * If you want more information available in the callback, like the client + * If you want more information available in the callback, like the qos event * or other information, you may use a lambda with captures or std::bind. * - * Note: this function must be overridden with a proper implementation - * by the custom classes who inherit from rclcpp::Waitable if they want to use it. - * - * \sa rclcpp::QOSEventHandlerBase::clear_on_ready_callback * \sa rmw_event_set_callback * \sa rcl_event_set_callback * diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 0eef8c75e8..8bb6c1fa76 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -148,13 +148,11 @@ class ServiceBase * If you want more information available in the callback, like the service * or other information, you may use a lambda with captures or std::bind. * - * \sa rclcpp::ServiceBase::clear_on_new_request_callback * \sa rmw_service_set_on_new_request_callback * \sa rcl_service_set_on_new_request_callback * * \param[in] callback functor to be called when a new request is received */ - RCLCPP_PUBLIC void set_on_new_request_callback(std::function callback) { @@ -203,7 +201,6 @@ class ServiceBase } /// Unset the callback registered for new requests, if any. - RCLCPP_PUBLIC void clear_on_new_request_callback() { diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index b040cad543..76dd54ff31 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -214,7 +214,7 @@ class Waitable * The application should provide a generic callback function that will be then * forwarded by the waitable to all of its entities. * Before forwarding, a different value for the identifier argument will be - * bounded to the function. + * bond to the function. * This implies that the provided callback can use the identifier to behave * differently depending on which entity triggered the waitable to become ready. * From bd2c5e4e570a258bb7514c9b146cbac9beb4bf31 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 15:18:26 +0100 Subject: [PATCH 11/31] fix comments Signed-off-by: Alberto Soragna --- .../rclcpp/experimental/subscription_intra_process_base.hpp | 1 - rclcpp/include/rclcpp/qos_event.hpp | 2 -- rclcpp/include/rclcpp/waitable.hpp | 4 ++-- 3 files changed, 2 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index a3dfba83af..f8afcb8825 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -154,7 +154,6 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable } /// Unset the callback registered for new messages, if any. - RCLCPP_PUBLIC void clear_on_ready_callback() override { diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 748662a73b..439917aa1e 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -147,7 +147,6 @@ class QOSEventHandlerBase : public Waitable * * \param[in] callback functor to be called when a new event occurs */ - RCLCPP_PUBLIC void set_on_ready_callback(std::function callback) override { @@ -198,7 +197,6 @@ class QOSEventHandlerBase : public Waitable } /// Unset the callback registered for new events, if any. - RCLCPP_PUBLIC void clear_on_ready_callback() override { diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 76dd54ff31..9ddc8a0882 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -203,9 +203,9 @@ class Waitable /// Set a callback to be called whenever the waitable becomes ready. /** - * The callback receives a size_t which is the number of responses received + * The callback receives a size_t which is the number of times the waitable was ready * since the last time this callback was called. - * Normally this is 1, but can be > 1 if responses were received before any + * Normally this is 1, but can be > 1 if waitable was triggered before any * callback was set. * * The callback also receives an int identifier argument. From 8574237a8cb58a63da99533be54a37a4cb062fa8 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 19:01:51 +0100 Subject: [PATCH 12/31] expose qos listener APIs from pub and sub; add unit-tests Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/publisher_base.hpp | 77 ++++++++++++++++++- rclcpp/include/rclcpp/subscription_base.hpp | 75 +++++++++++++++++- rclcpp/include/rclcpp/wait_set_template.hpp | 6 +- .../rclcpp/node_interfaces/node_topics.cpp | 6 +- rclcpp/src/rclcpp/publisher_base.cpp | 4 +- rclcpp/src/rclcpp/subscription_base.cpp | 7 +- rclcpp/test/rclcpp/test_publisher.cpp | 3 +- rclcpp/test/rclcpp/test_qos_event.cpp | 66 ++++++++++++++++ rclcpp/test/rclcpp/test_wait_set.cpp | 4 +- 9 files changed, 230 insertions(+), 18 deletions(-) diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 01bdbb585f..56292f1fbe 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -24,6 +24,8 @@ #include #include #include +#include +#include #include #include "rcl/publisher.h" @@ -112,9 +114,10 @@ class PublisherBase : public std::enable_shared_from_this get_publisher_handle() const; /// Get all the QoS event handlers associated with this publisher. - /** \return The vector of QoS event handlers. */ + /** \return The map of QoS event handlers. */ RCLCPP_PUBLIC - const std::vector> & + const + std::unordered_map> & get_event_handlers() const; /// Get subscription count @@ -245,6 +248,71 @@ class PublisherBase : public std::enable_shared_from_this } } + /// Set a callback to be called when each new qos event instance occurs. + /** + * The callback receives a size_t which is the number of events that occurred + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if events occurred before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the qos event + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback + * + * \param[in] callback functor to be called when a new event occurs + * \param[in] event_type identifier for the qos event we want to attach the callback to + */ + void + set_on_new_qos_event_callback( + std::function callback, + rcl_publisher_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling set_on_new_qos_event_callback for non registered event_type"); + return; + } + + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_qos_event_callback " + "is not callable."); + } + + // The on_ready_callback signature has an extra `int` argument used to disambiguate between + // possible different entities within a generic waitable. + // We hide that detail to users of this method. + std::function new_callback = std::bind(callback, std::placeholders::_1); + event_handlers_[event_type]->set_on_ready_callback(new_callback); + } + + /// Unset the callback registered for new qos events, if any. + void + clear_on_new_qos_event_callback(rcl_publisher_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling clear_on_new_qos_event_callback for non registered event_type"); + return; + } + + event_handlers_[event_type]->clear_on_ready_callback(); + } + protected: template void @@ -258,7 +326,7 @@ class PublisherBase : public std::enable_shared_from_this rcl_publisher_event_init, publisher_handle_, event_type); - event_handlers_.emplace_back(handler); + event_handlers_.insert(std::make_pair(event_type, handler)); } RCLCPP_PUBLIC @@ -268,7 +336,8 @@ class PublisherBase : public std::enable_shared_from_this std::shared_ptr publisher_handle_; - std::vector> event_handlers_; + std::unordered_map> event_handlers_; using IntraProcessManagerWeakPtr = std::weak_ptr; diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 807083c173..f6ec6876bc 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -102,9 +102,10 @@ class SubscriptionBase : public std::enable_shared_from_this get_subscription_handle() const; /// Get all the QoS event handlers associated with this subscription. - /** \return The vector of QoS event handlers. */ + /** \return The map of QoS event handlers. */ RCLCPP_PUBLIC - const std::vector> & + const + std::unordered_map> & get_event_handlers() const; /// Get the actual QoS settings, after the defaults have been determined. @@ -421,6 +422,71 @@ class SubscriptionBase : public std::enable_shared_from_this subscription_intra_process_->clear_on_ready_callback(); } + /// Set a callback to be called when each new qos event instance occurs. + /** + * The callback receives a size_t which is the number of events that occurred + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if events occurred before any + * callback was set. + * + * Since this callback is called from the middleware, you should aim to make + * it fast and not blocking. + * If you need to do a lot of work or wait for some other event, you should + * spin it off to another thread, otherwise you risk blocking the middleware. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the qos event + * or other information, you may use a lambda with captures or std::bind. + * + * \sa rclcpp::QOSEventHandlerBase::set_on_ready_callback + * + * \param[in] callback functor to be called when a new event occurs + * \param[in] event_type identifier for the qos event we want to attach the callback to + */ + void + set_on_new_qos_event_callback( + std::function callback, + rcl_subscription_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling set_on_new_qos_event_callback for non registered event_type"); + return; + } + + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_new_qos_event_callback " + "is not callable."); + } + + // The on_ready_callback signature has an extra `int` argument used to disambiguate between + // possible different entities within a generic waitable. + // We hide that detail to users of this method. + std::function new_callback = std::bind(callback, std::placeholders::_1); + event_handlers_[event_type]->set_on_ready_callback(new_callback); + } + + /// Unset the callback registered for new qos events, if any. + void + clear_on_new_qos_event_callback(rcl_subscription_event_type_t event_type) + { + if (event_handlers_.count(event_type) == 0) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Calling clear_on_new_qos_event_callback for non registered event_type"); + return; + } + + event_handlers_[event_type]->clear_on_ready_callback(); + } + protected: template void @@ -435,7 +501,7 @@ class SubscriptionBase : public std::enable_shared_from_this get_subscription_handle(), event_type); qos_events_in_use_by_wait_set_.insert(std::make_pair(handler.get(), false)); - event_handlers_.emplace_back(handler); + event_handlers_.insert(std::make_pair(event_type, handler)); } RCLCPP_PUBLIC @@ -456,7 +522,8 @@ class SubscriptionBase : public std::enable_shared_from_this std::shared_ptr intra_process_subscription_handle_; rclcpp::Logger node_logger_; - std::vector> event_handlers_; + std::unordered_map> event_handlers_; bool use_intra_process_; IntraProcessManagerWeakPtr weak_ipm_; diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 3adee41164..3654801c91 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -155,7 +155,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->storage_add_subscription(std::move(local_subscription)); } if (mask.include_events) { - for (auto event : inner_subscription->get_event_handlers()) { + for (auto key_event_pair : inner_subscription->get_event_handlers()) { + auto event = key_event_pair.second; auto local_subscription = inner_subscription; bool already_in_use = local_subscription->exchange_in_use_by_wait_set_state(event.get(), true); @@ -225,7 +226,8 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli this->storage_remove_subscription(std::move(local_subscription)); } if (mask.include_events) { - for (auto event : inner_subscription->get_event_handlers()) { + for (auto key_event_pair : inner_subscription->get_event_handlers()) { + auto event = key_event_pair.second; auto local_subscription = inner_subscription; local_subscription->exchange_in_use_by_wait_set_state(event.get(), false); this->storage_remove_waitable(std::move(event)); diff --git a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp index fa188a2f9a..c353c5a481 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_topics.cpp @@ -55,7 +55,8 @@ NodeTopics::add_publisher( callback_group = node_base_->get_default_callback_group(); } - for (auto & publisher_event : publisher->get_event_handlers()) { + for (auto & key_event_pair : publisher->get_event_handlers()) { + auto publisher_event = key_event_pair.second; callback_group->add_waitable(publisher_event); } @@ -96,7 +97,8 @@ NodeTopics::add_subscription( callback_group->add_subscription(subscription); - for (auto & subscription_event : subscription->get_event_handlers()) { + for (auto & key_event_pair : subscription->get_event_handlers()) { + auto subscription_event = key_event_pair.second; callback_group->add_waitable(subscription_event); } diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 17d72594d5..d820234842 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include "rcutils/logging_macros.h" @@ -154,7 +155,8 @@ PublisherBase::get_publisher_handle() const return publisher_handle_; } -const std::vector> & +const +std::unordered_map> & PublisherBase::get_event_handlers() const { return event_handlers_; diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 8dd5c36086..0a9ab33c07 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include "rclcpp/exceptions.hpp" @@ -116,7 +117,8 @@ SubscriptionBase::get_subscription_handle() const return subscription_handle_; } -const std::vector> & +const +std::unordered_map> & SubscriptionBase::get_event_handlers() const { return event_handlers_; @@ -283,7 +285,8 @@ SubscriptionBase::exchange_in_use_by_wait_set_state( if (get_intra_process_waitable().get() == pointer_to_subscription_part) { return intra_process_subscription_waitable_in_use_by_wait_set_.exchange(in_use_state); } - for (const auto & qos_event : event_handlers_) { + for (const auto & key_event_pair : event_handlers_) { + auto qos_event = key_event_pair.second; if (qos_event.get() == pointer_to_subscription_part) { return qos_events_in_use_by_wait_set_[qos_event.get()].exchange(in_use_state); } diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 3be64e9647..1509e3f487 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -503,7 +503,8 @@ TEST_F(TestPublisher, run_event_handlers) { initialize(); auto publisher = node->create_publisher("topic", 10); - for (const auto & handler : publisher->get_event_handlers()) { + for (const auto & key_event_pair : publisher->get_event_handlers()) { + auto handler = key_event_pair.second; std::shared_ptr data = handler->take_data(); handler->execute(data); } diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index ebf0e14afb..97a36b512d 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -27,6 +27,8 @@ #include "../mocking_utils/patch.hpp" +using namespace std::chrono_literals; + class TestQosEvent : public ::testing::Test { protected: @@ -325,3 +327,67 @@ TEST_F(TestQosEvent, add_to_wait_set) { EXPECT_THROW(handler.add_to_wait_set(&wait_set), rclcpp::exceptions::RCLError); } } + +TEST_F(TestQosEvent, test_on_new_event_callback) +{ + rclcpp::QoS qos_profile_publisher(10); + qos_profile_publisher.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + auto publisher = node->create_publisher( + topic_name, qos_profile_publisher); + + std::atomic c1 {0}; + auto increase_c1_cb = [&c1](size_t count_events) {c1 += count_events;}; + publisher->set_on_new_qos_event_callback(increase_c1_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + + rclcpp::QoS qos_profile_subscription(10); + qos_profile_subscription.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + auto subscription = node->create_subscription( + topic_name, qos_profile_subscription, message_callback); + + auto start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1, 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_events) {c2 += count_events;}; + subscription->set_on_new_qos_event_callback( + increase_c2_cb, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1, 1u); + EXPECT_EQ(c2, 1u); + + auto publisher2 = node->create_publisher( + topic_name, qos_profile_publisher); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c2 == 1u && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1, 1u); + EXPECT_EQ(c2, 2u); + + publisher->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + + auto subscription2 = node->create_subscription( + topic_name, qos_profile_subscription, message_callback); + + publisher->set_on_new_qos_event_callback(increase_c1_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + + start = std::chrono::steady_clock::now(); + do { + std::this_thread::sleep_for(100ms); + } while (c1 == 1 && std::chrono::steady_clock::now() - start < 10s); + + EXPECT_EQ(c1, 2u); + EXPECT_EQ(c2, 2u); +} diff --git a/rclcpp/test/rclcpp/test_wait_set.cpp b/rclcpp/test/rclcpp/test_wait_set.cpp index f9d97a5759..bf7ee26ed0 100644 --- a/rclcpp/test/rclcpp/test_wait_set.cpp +++ b/rclcpp/test/rclcpp/test_wait_set.cpp @@ -257,7 +257,7 @@ TEST_F(TestWaitSet, add_guard_condition_to_two_different_wait_set) { rclcpp::PublisherOptions po; po.event_callbacks.deadline_callback = [](rclcpp::QOSDeadlineOfferedInfo &) {}; auto pub = node->create_publisher("~/test", 1, po); - auto qos_event = pub->get_event_handlers()[0]; + auto qos_event = pub->get_event_handlers().begin()->second; wait_set1.add_waitable(qos_event, pub); ASSERT_THROW( { @@ -301,7 +301,7 @@ TEST_F(TestWaitSet, add_remove_wait) { [](rclcpp::QOSDeadlineOfferedInfo &) {}; auto pub = node->create_publisher( "~/test", 1, publisher_options); - auto qos_event = pub->get_event_handlers()[0]; + auto qos_event = pub->get_event_handlers().begin()->second; // Subscription mask is required here for coverage. wait_set.add_subscription(sub, {true, true, true}); From 2bd63c048ab61130b00e0a653c71a5aa4950ce2f Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Fri, 23 Apr 2021 19:15:27 +0100 Subject: [PATCH 13/31] add qos event unit-test for invalid callbacks Signed-off-by: Alberto Soragna --- rclcpp/test/rclcpp/test_qos_event.cpp | 28 +++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index 97a36b512d..bd6a0e77ed 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -391,3 +391,31 @@ TEST_F(TestQosEvent, test_on_new_event_callback) EXPECT_EQ(c1, 2u); EXPECT_EQ(c2, 2u); } + +TEST_F(TestQosEvent, test_invalid_on_new_event_callback) +{ + auto pub = node->create_publisher(topic_name, 10); + auto sub = node->create_subscription(topic_name, 10, message_callback); + auto dummy_cb = [](size_t count_events) {(void)count_events;}; + std::function invalid_cb; + + EXPECT_NO_THROW( + pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS)); + + EXPECT_NO_THROW( + pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS)); + + EXPECT_NO_THROW( + sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS)); + + EXPECT_NO_THROW( + sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS)); + + EXPECT_THROW( + pub->set_on_new_qos_event_callback(invalid_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS), + std::invalid_argument); + + EXPECT_THROW( + sub->set_on_new_qos_event_callback(invalid_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS), + std::invalid_argument); +} From c1ccb94e9aab80c6e5b61f5375162d6d7dbe342d Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 28 Apr 2021 14:36:15 -0300 Subject: [PATCH 14/31] Use QoS depth to limit callbacks count Signed-off-by: Mauro Passerino --- .../experimental/subscription_intra_process_base.hpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index f8afcb8825..a4b2933de6 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -147,8 +147,12 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable on_new_message_callback_ = new_callback; if (unread_count_ > 0) { - // Use qos profile depth as upper bound for unread_count_ - on_new_message_callback_(std::min(unread_count_, qos_profile_.depth)); + if (qos_profile_.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL || qos_profile_.depth == 0) { + on_new_message_callback_(unread_count_); + } else { + // Use qos profile depth as upper bound for unread_count_ + on_new_message_callback_(std::min(unread_count_, qos_profile_.depth)); + } unread_count_ = 0; } } From e7784493e63a4711ed433ed2150896d693c584d7 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Wed, 2 Jun 2021 16:22:05 +0100 Subject: [PATCH 15/31] fix ipc-subscription Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/subscription.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 70d2676d3c..7d079e3c9c 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -197,6 +197,11 @@ class Subscription : public SubscriptionBase "intraprocess communication allowed only with volatile durability"); } + using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< + CallbackMessageT, + AllocatorT, + typename MessageUniquePtr::deleter_type>; + // First create a SubscriptionIntraProcess which will be given to the intra-process manager. auto context = node_base->get_context(); using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< From 1bdc01d5155a133e7167ecea58a89ec66598c524 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 14 Jul 2021 12:09:10 +0100 Subject: [PATCH 16/31] Rename CallbackMessageT -> ROSMessageType Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/subscription.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 7d079e3c9c..2fb9b3c116 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -198,7 +198,7 @@ class Subscription : public SubscriptionBase } using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< - CallbackMessageT, + ROSMessageType, AllocatorT, typename MessageUniquePtr::deleter_type>; From 2d51d087fa04e2d0598883beadb4d593d0e5ae8a Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 14 Jul 2021 10:48:08 +0100 Subject: [PATCH 17/31] Set callbacks to Actions Signed-off-by: Mauro Passerino --- .../include/rclcpp_action/client.hpp | 65 ++++++++ .../include/rclcpp_action/server.hpp | 62 +++++++ rclcpp_action/src/client.cpp | 152 ++++++++++++++++++ rclcpp_action/src/server.cpp | 130 +++++++++++++++ 4 files changed, 409 insertions(+) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index ed590247d9..74cadf6e6f 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include "rclcpp_action/client_goal_handle.hpp" @@ -62,6 +63,16 @@ class ClientBase : public rclcpp::Waitable RCLCPP_ACTION_PUBLIC virtual ~ClientBase(); + /// Enum to identify entities belonging to the action client + enum class EntityType + { + GoalClient, + ResultClient, + CancelClient, + FeedbackSubscription, + StatusSubscription, + }; + /// Return true if there is an action server that is ready to take goal requests. RCLCPP_ACTION_PUBLIC bool @@ -126,6 +137,39 @@ class ClientBase : public rclcpp::Waitable void execute(std::shared_ptr & data) override; + /// \internal + /// Set a callback to be called when action client entities have an event + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * The callback also receives an int identifier argument, which identifies + * the action client entity which is ready. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when a new message is received. + */ + RCLCPP_ACTION_PUBLIC + void + set_on_ready_callback(std::function callback) override; + + /// Unset the callback registered for new events, if any. + RCLCPP_ACTION_PUBLIC + void + clear_on_ready_callback() override; + // End Waitables API // ----------------- @@ -244,8 +288,29 @@ class ClientBase : public rclcpp::Waitable // End API for communication between ClientBase and Client<> // --------------------------------------------------------- + /// \internal + /// Set a callback to be called when the specified entity is ready + RCLCPP_ACTION_PUBLIC + void + set_on_ready_callback( + EntityType entity_type, + rcl_event_callback_t callback, + const void * user_data); + + // Mutex to protect the callbacks storage. + std::recursive_mutex reentrant_mutex_; + // Storage for std::function callbacks to keep them in scope + std::unordered_map> entity_type_to_on_ready_callback_; + private: std::unique_ptr pimpl_; + + /// Set a std::function callback to be called when the specified entity is ready + RCLCPP_ACTION_PUBLIC + void + set_callback_to_entity( + EntityType entity_type, + std::function callback); }; /// Action Client diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 63c99d51e0..67c495d2b7 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -70,6 +70,14 @@ enum class CancelResponse : int8_t class ServerBase : public rclcpp::Waitable { public: + /// Enum to identify entities belonging to the action server + enum class EntityType + { + GoalService, + ResultService, + CancelService, + }; + RCLCPP_ACTION_PUBLIC virtual ~ServerBase(); @@ -128,6 +136,39 @@ class ServerBase : public rclcpp::Waitable void execute(std::shared_ptr & data) override; + /// \internal + /// Set a callback to be called when action server entities have an event + /** + * The callback receives a size_t which is the number of messages received + * since the last time this callback was called. + * Normally this is 1, but can be > 1 if messages were received before any + * callback was set. + * + * The callback also receives an int identifier argument, which identifies + * the action server entity which is ready. + * This implies that the provided callback can use the identifier to behave + * differently depending on which entity triggered the waitable to become ready. + * + * Calling it again will clear any previously set callback. + * + * An exception will be thrown if the callback is not callable. + * + * This function is thread-safe. + * + * If you want more information available in the callback, like the subscription + * or other information, you may use a lambda with captures or std::bind. + * + * \param[in] callback functor to be called when a new message is received. + */ + RCLCPP_ACTION_PUBLIC + void + set_on_ready_callback(std::function callback) override; + + /// Unset the callback to be called whenever the waitable becomes ready. + RCLCPP_ACTION_PUBLIC + void + clear_on_ready_callback() override; + // End Waitables API // ----------------- @@ -256,6 +297,27 @@ class ServerBase : public rclcpp::Waitable /// Private implementation /// \internal std::unique_ptr pimpl_; + + /// Set a std::function callback to be called when the specified entity is ready + RCLCPP_ACTION_PUBLIC + void + set_callback_to_entity( + EntityType entity_type, + std::function callback); + +protected: + // Mutex to protect the callbacks storage. + std::recursive_mutex reentrant_mutex_; + // Storage for std::function callbacks to keep them in scope + std::unordered_map> entity_type_to_on_ready_callback_; + + /// Set a callback to be called when the specified entity is ready + RCLCPP_ACTION_PUBLIC + void + set_on_ready_callback( + EntityType entity_type, + rcl_event_callback_t callback, + const void * user_data); }; /// Action Server diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 2f39a50e52..e056e657a4 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -136,6 +136,7 @@ ClientBase::ClientBase( ClientBase::~ClientBase() { + clear_on_ready_callback(); } bool @@ -385,6 +386,157 @@ ClientBase::generate_goal_id() return goal_id; } +void +ClientBase::set_on_ready_callback(std::function callback) +{ + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + set_callback_to_entity(EntityType::GoalClient, callback); + set_callback_to_entity(EntityType::ResultClient, callback); + set_callback_to_entity(EntityType::CancelClient, callback); + set_callback_to_entity(EntityType::FeedbackSubscription, callback); + set_callback_to_entity(EntityType::StatusSubscription, callback); +} + +void +ClientBase::set_callback_to_entity( + EntityType entity_type, + std::function callback) +{ + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, entity_type, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(entity_type)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + pimpl_->logger, + "rclcpp_action::ClientBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + pimpl_->logger, + "rclcpp_action::ClientBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_ready_callback( + entity_type, + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + std::lock_guard lock(reentrant_mutex_); + // Store the std::function to keep it in scope, also overwrites the existing one. + auto it = entity_type_to_on_ready_callback_.find(entity_type); + + if (it != entity_type_to_on_ready_callback_.end()) { + it->second = new_callback; + } else { + entity_type_to_on_ready_callback_.emplace(entity_type, new_callback); + } + + // Set it again, now using the permanent storage. + it = entity_type_to_on_ready_callback_.find(entity_type); + + if (it != entity_type_to_on_ready_callback_.end()) { + auto & cb = it->second; + set_on_ready_callback( + entity_type, + rclcpp::detail::cpp_callback_trampoline, + static_cast(&cb)); + } +} + +void +ClientBase::set_on_ready_callback( + EntityType entity_type, + rcl_event_callback_t callback, + const void * user_data) +{ + rcl_ret_t ret = RCL_RET_ERROR; + + switch (entity_type) { + case EntityType::GoalClient: + { + ret = rcl_action_client_set_goal_client_callback( + pimpl_->client_handle.get(), + callback, + user_data); + break; + } + + case EntityType::ResultClient: + { + ret = rcl_action_client_set_result_client_callback( + pimpl_->client_handle.get(), + callback, + user_data); + break; + } + + case EntityType::CancelClient: + { + ret = rcl_action_client_set_cancel_client_callback( + pimpl_->client_handle.get(), + callback, + user_data); + break; + } + + case EntityType::FeedbackSubscription: + { + ret = rcl_action_client_set_feedback_subscription_callback( + pimpl_->client_handle.get(), + callback, + user_data); + break; + } + + case EntityType::StatusSubscription: + { + ret = rcl_action_client_set_status_subscription_callback( + pimpl_->client_handle.get(), + callback, + user_data); + break; + } + + default: + throw std::runtime_error("ClientBase::set_on_ready_callback: Unknown entity type."); + break; + } + + if (RCL_RET_OK != ret) { + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on ready callback for action client"); + } +} + +void +ClientBase::clear_on_ready_callback() +{ + set_on_ready_callback(EntityType::GoalClient, nullptr, nullptr); + set_on_ready_callback(EntityType::ResultClient, nullptr, nullptr); + set_on_ready_callback(EntityType::CancelClient, nullptr, nullptr); + set_on_ready_callback(EntityType::FeedbackSubscription, nullptr, nullptr); + set_on_ready_callback(EntityType::StatusSubscription, nullptr, nullptr); + + std::lock_guard lock(reentrant_mutex_); + entity_type_to_on_ready_callback_.clear(); +} + std::shared_ptr ClientBase::take_data() { diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index e4765e9461..d2d34b50b8 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -132,6 +132,7 @@ ServerBase::ServerBase( ServerBase::~ServerBase() { + clear_on_ready_callback(); } size_t @@ -678,3 +679,132 @@ ServerBase::publish_feedback(std::shared_ptr feedback_msg) rclcpp::exceptions::throw_from_rcl_error(ret, "Failed to publish feedback"); } } + +void +ServerBase::set_on_ready_callback(std::function callback) +{ + if (!callback) { + throw std::invalid_argument( + "The callback passed to set_on_ready_callback " + "is not callable."); + } + + set_callback_to_entity(EntityType::GoalService, callback); + set_callback_to_entity(EntityType::ResultService, callback); + set_callback_to_entity(EntityType::CancelService, callback); +} + +void +ServerBase::set_callback_to_entity( + EntityType entity_type, + std::function callback) +{ + // Note: we bind the int identifier argument to this waitable's entity types + auto new_callback = + [callback, entity_type, this](size_t number_of_events) { + try { + callback(number_of_events, static_cast(entity_type)); + } catch (const std::exception & exception) { + RCLCPP_ERROR_STREAM( + pimpl_->logger_, + "rclcpp_action::ServerBase@" << this << + " caught " << rmw::impl::cpp::demangle(exception) << + " exception in user-provided callback for the 'on ready' callback: " << + exception.what()); + } catch (...) { + RCLCPP_ERROR_STREAM( + pimpl_->logger_, + "rclcpp_action::ServerBase@" << this << + " caught unhandled exception in user-provided callback " << + "for the 'on ready' callback"); + } + }; + + + // Set it temporarily to the new callback, while we replace the old one. + // This two-step setting, prevents a gap where the old std::function has + // been replaced but the middleware hasn't been told about the new one yet. + set_on_ready_callback( + entity_type, + rclcpp::detail::cpp_callback_trampoline, + static_cast(&new_callback)); + + std::lock_guard lock(reentrant_mutex_); + // Store the std::function to keep it in scope, also overwrites the existing one. + auto it = entity_type_to_on_ready_callback_.find(entity_type); + + if (it != entity_type_to_on_ready_callback_.end()) { + it->second = new_callback; + } else { + entity_type_to_on_ready_callback_.emplace(entity_type, new_callback); + } + + // Set it again, now using the permanent storage. + it = entity_type_to_on_ready_callback_.find(entity_type); + + if (it != entity_type_to_on_ready_callback_.end()) { + auto & cb = it->second; + set_on_ready_callback( + entity_type, + rclcpp::detail::cpp_callback_trampoline, + static_cast(&cb)); + } +} + +void +ServerBase::set_on_ready_callback( + EntityType entity_type, + rcl_event_callback_t callback, + const void * user_data) +{ + rcl_ret_t ret = RCL_RET_ERROR; + + switch (entity_type) { + case EntityType::GoalService: + { + ret = rcl_action_server_set_goal_service_callback( + pimpl_->action_server_.get(), + callback, + user_data); + break; + } + + case EntityType::ResultService: + { + ret = rcl_action_server_set_result_service_callback( + pimpl_->action_server_.get(), + callback, + user_data); + break; + } + + case EntityType::CancelService: + { + ret = rcl_action_server_set_cancel_service_callback( + pimpl_->action_server_.get(), + callback, + user_data); + break; + } + + default: + throw std::runtime_error("ServerBase::set_on_ready_callback: Unknown entity type."); + break; + } + + if (RCL_RET_OK != ret) { + using rclcpp::exceptions::throw_from_rcl_error; + throw_from_rcl_error(ret, "failed to set the on ready callback for action client"); + } +} + +void +ServerBase::clear_on_ready_callback() +{ + set_on_ready_callback(EntityType::GoalService, nullptr, nullptr); + set_on_ready_callback(EntityType::ResultService, nullptr, nullptr); + set_on_ready_callback(EntityType::CancelService, nullptr, nullptr); + + std::lock_guard lock(reentrant_mutex_); + entity_type_to_on_ready_callback_.clear(); +} From 8da344928f180e8227bd07d31a536c5271181efb Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 26 Jan 2022 01:02:30 -0800 Subject: [PATCH 18/31] changes from upstream Signed-off-by: William Woodall --- .../rclcpp/experimental/subscription_intra_process_base.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index a4b2933de6..022ceff7ad 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -60,7 +60,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable get_number_of_ready_guard_conditions() override {return 1;} RCLCPP_PUBLIC - bool + void add_to_wait_set(rcl_wait_set_t * wait_set) override; bool @@ -147,11 +147,11 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable on_new_message_callback_ = new_callback; if (unread_count_ > 0) { - if (qos_profile_.history == RMW_QOS_POLICY_HISTORY_KEEP_ALL || qos_profile_.depth == 0) { + if (qos_profile_.history() == HistoryPolicy::KeepLast || qos_profile_.depth() == 0) { on_new_message_callback_(unread_count_); } else { // Use qos profile depth as upper bound for unread_count_ - on_new_message_callback_(std::min(unread_count_, qos_profile_.depth)); + on_new_message_callback_(std::min(unread_count_, qos_profile_.depth())); } unread_count_ = 0; } From 1804a6038f2aa66ff4d75c491d60fb95214b70f4 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 2 Feb 2022 13:42:13 -0300 Subject: [PATCH 19/31] Unset callback on entities destruction Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/client.hpp | 6 ++++-- .../experimental/subscription_intra_process_base.hpp | 2 +- rclcpp/include/rclcpp/qos_event.hpp | 6 ++++-- rclcpp/include/rclcpp/service.hpp | 6 ++++-- rclcpp/include/rclcpp/subscription_base.hpp | 7 +++++-- rclcpp/src/rclcpp/client.cpp | 1 + rclcpp/src/rclcpp/publisher_base.cpp | 5 +++++ rclcpp/src/rclcpp/qos_event.cpp | 6 +++++- rclcpp/src/rclcpp/service.cpp | 4 +++- rclcpp/src/rclcpp/subscription_base.cpp | 7 +++++++ rclcpp/src/rclcpp/subscription_intra_process_base.cpp | 5 +++++ rclcpp/src/rclcpp/waitable.cpp | 4 ++-- 12 files changed, 46 insertions(+), 13 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 8da52f1675..91ab2c6333 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -295,8 +295,10 @@ class ClientBase clear_on_new_response_callback() { std::lock_guard lock(reentrant_mutex_); - set_on_new_response_callback(nullptr, nullptr); - on_new_response_callback_ = nullptr; + if (on_new_response_callback_) { + set_on_new_response_callback(nullptr, nullptr); + on_new_response_callback_ = nullptr; + } } protected: diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index eac6648fa4..44b58c5201 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -50,7 +50,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable : gc_(context), topic_name_(topic_name), qos_profile_(qos_profile) {} - virtual ~SubscriptionIntraProcessBase() = default; + virtual ~SubscriptionIntraProcessBase(); RCLCPP_PUBLIC size_t diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 439917aa1e..b32e79c464 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -201,8 +201,10 @@ class QOSEventHandlerBase : public Waitable clear_on_ready_callback() override { std::lock_guard lock(reentrant_mutex_); - set_on_new_event_callback(nullptr, nullptr); - on_new_event_callback_ = nullptr; + if (on_new_event_callback_) { + set_on_new_event_callback(nullptr, nullptr); + on_new_event_callback_ = nullptr; + } } protected: diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 8bb6c1fa76..99680297ed 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -205,8 +205,10 @@ class ServiceBase clear_on_new_request_callback() { std::lock_guard lock(reentrant_mutex_); - set_on_new_request_callback(nullptr, nullptr); - on_new_request_callback_ = nullptr; + if (on_new_request_callback_) { + set_on_new_request_callback(nullptr, nullptr); + on_new_request_callback_ = nullptr; + } } protected: diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index f6ec6876bc..a464694cee 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -363,8 +363,11 @@ class SubscriptionBase : public std::enable_shared_from_this clear_on_new_message_callback() { std::lock_guard lock(reentrant_mutex_); - set_on_new_message_callback(nullptr, nullptr); - on_new_message_callback_ = nullptr; + + if (on_new_message_callback_) { + set_on_new_message_callback(nullptr, nullptr); + on_new_message_callback_ = nullptr; + } } /// Set a callback to be called when each new intra-process message is received. diff --git a/rclcpp/src/rclcpp/client.cpp b/rclcpp/src/rclcpp/client.cpp index e340a7d019..2380b890f5 100644 --- a/rclcpp/src/rclcpp/client.cpp +++ b/rclcpp/src/rclcpp/client.cpp @@ -67,6 +67,7 @@ ClientBase::ClientBase( ClientBase::~ClientBase() { + clear_on_new_response_callback(); // Make sure the client handle is destructed as early as possible and before the node handle client_handle_.reset(); } diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index d820234842..55d7897214 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -100,6 +100,11 @@ PublisherBase::PublisherBase( PublisherBase::~PublisherBase() { + for (const auto & pair : event_handlers_ ) { + rcl_publisher_event_type_t event_type = pair.first; + clear_on_new_qos_event_callback(event_type); + } + // must fini the events before fini-ing the publisher event_handlers_.clear(); diff --git a/rclcpp/src/rclcpp/qos_event.cpp b/rclcpp/src/rclcpp/qos_event.cpp index aa35199501..8bfd5b3304 100644 --- a/rclcpp/src/rclcpp/qos_event.cpp +++ b/rclcpp/src/rclcpp/qos_event.cpp @@ -35,6 +35,10 @@ UnsupportedEventTypeException::UnsupportedEventTypeException( QOSEventHandlerBase::~QOSEventHandlerBase() { + if (on_new_event_callback_) { + clear_on_ready_callback(); + } + if (rcl_event_fini(&event_handle_) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( "rclcpp", @@ -79,7 +83,7 @@ QOSEventHandlerBase::set_on_new_event_callback( if (RCL_RET_OK != ret) { using rclcpp::exceptions::throw_from_rcl_error; - throw_from_rcl_error(ret, "failed to set the on new message callback for subscription"); + throw_from_rcl_error(ret, "failed to set the on new message callback for QOS Event"); } } diff --git a/rclcpp/src/rclcpp/service.cpp b/rclcpp/src/rclcpp/service.cpp index bfd9aba0c0..d2f333cacf 100644 --- a/rclcpp/src/rclcpp/service.cpp +++ b/rclcpp/src/rclcpp/service.cpp @@ -33,7 +33,9 @@ ServiceBase::ServiceBase(std::shared_ptr node_handle) {} ServiceBase::~ServiceBase() -{} +{ + clear_on_new_request_callback(); +} bool ServiceBase::take_type_erased_request(void * request_out, rmw_request_id_t & request_id_out) diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 0a9ab33c07..b85ae34b1e 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -85,6 +85,13 @@ SubscriptionBase::SubscriptionBase( SubscriptionBase::~SubscriptionBase() { + clear_on_new_message_callback(); + + for (const auto & pair : event_handlers_ ) { + rcl_subscription_event_type_t event_type = pair.first; + clear_on_new_qos_event_callback(event_type); + } + if (!use_intra_process_) { return; } diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index b13123b65b..21ccb433ff 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -17,6 +17,11 @@ using rclcpp::experimental::SubscriptionIntraProcessBase; +SubscriptionIntraProcessBase::~SubscriptionIntraProcessBase() +{ + clear_on_ready_callback(); +} + void SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) { diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index a42968dc95..16ebb1b546 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -74,6 +74,6 @@ void Waitable::clear_on_ready_callback() { throw std::runtime_error( - "Custom waitables should override clear_on_ready_callback " - "if they want to use it."); + "Custom waitables should override clear_on_ready_callback if they " + "want to use it and make sure to call it on the waitable destructor."); } From 6d4748ed604db222b16054881067ad18792b4925 Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Wed, 9 Feb 2022 14:59:50 +0000 Subject: [PATCH 20/31] fix rebase errors Signed-off-by: Alberto Soragna --- .../subscription_intra_process.hpp | 41 ------------------- rclcpp/include/rclcpp/subscription.hpp | 9 ---- 2 files changed, 50 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index fffe197ddb..803d940086 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -124,47 +124,6 @@ class SubscriptionIntraProcess execute_impl(data); } - void - provide_intra_process_message(ConstMessageSharedPtr message) - { - buffer_->add_shared(std::move(message)); - invoke_callback(); - trigger_guard_condition(); - } - - void - provide_intra_process_message(MessageUniquePtr message) - { - buffer_->add_unique(std::move(message)); - invoke_callback(); - trigger_guard_condition(); - } - - bool - use_take_shared_method() const - { - return buffer_->use_take_shared_method(); - } - -private: - void - invoke_callback() - { - std::lock_guard lock(reentrant_mutex_); - if (on_new_message_callback_) { - on_new_message_callback_(1); - } else { - unread_count_++; - } - } - - void - trigger_guard_condition() - { - rcl_ret_t ret = rcl_trigger_guard_condition(&gc_); - (void)ret; - } - protected: template typename std::enable_if::value, void>::type diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 2fb9b3c116..2d52388e3b 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -197,17 +197,8 @@ class Subscription : public SubscriptionBase "intraprocess communication allowed only with volatile durability"); } - using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< - ROSMessageType, - AllocatorT, - typename MessageUniquePtr::deleter_type>; - // First create a SubscriptionIntraProcess which will be given to the intra-process manager. auto context = node_base->get_context(); - using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< - CallbackMessageT, - AllocatorT, - typename MessageUniquePtr::deleter_type>; subscription_intra_process_ = std::make_shared( callback, options_.get_allocator(), From 82a1b465246f1cbde586474fd90e1fb0389edd1a Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Wed, 9 Feb 2022 18:16:02 +0000 Subject: [PATCH 21/31] fix unit-tests Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/client.hpp | 6 +- .../subscription_intra_process_base.hpp | 7 +- .../subscription_intra_process_buffer.hpp | 16 +++ rclcpp/include/rclcpp/publisher_base.hpp | 2 +- rclcpp/include/rclcpp/qos_event.hpp | 6 +- rclcpp/include/rclcpp/service.hpp | 6 +- rclcpp/include/rclcpp/subscription.hpp | 17 ++- rclcpp/include/rclcpp/subscription_base.hpp | 8 +- rclcpp/src/rclcpp/publisher_base.cpp | 2 +- rclcpp/src/rclcpp/subscription_base.cpp | 2 +- rclcpp/test/rclcpp/test_qos_event.cpp | 109 ++++++++++-------- .../include/rclcpp_action/client.hpp | 2 +- .../include/rclcpp_action/server.hpp | 2 +- rclcpp_action/src/client.cpp | 4 +- rclcpp_action/src/server.cpp | 4 +- 15 files changed, 110 insertions(+), 83 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index 91ab2c6333..b0da172559 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -272,7 +272,7 @@ class ClientBase } }; - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has @@ -294,7 +294,7 @@ class ClientBase void clear_on_new_response_callback() { - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); if (on_new_response_callback_) { set_on_new_response_callback(nullptr, nullptr); on_new_response_callback_ = nullptr; @@ -329,7 +329,7 @@ class ClientBase std::atomic in_use_by_wait_set_{false}; - std::recursive_mutex reentrant_mutex_; + std::recursive_mutex listener_mutex_; std::function on_new_response_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 44b58c5201..4469c92031 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ #define RCLCPP__EXPERIMENTAL__SUBSCRIPTION_INTRA_PROCESS_BASE_HPP_ +#include #include #include #include @@ -140,7 +141,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable } }; - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); on_new_message_callback_ = new_callback; if (unread_count_ > 0) { @@ -158,12 +159,12 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable void clear_on_ready_callback() override { - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); on_new_message_callback_ = nullptr; } protected: - std::recursive_mutex reentrant_mutex_; + std::recursive_mutex listener_mutex_; std::function on_new_message_callback_ {nullptr}; size_t unread_count_{0}; rclcpp::GuardCondition gc_; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index ecb02f501d..3f7dc48fa6 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -125,6 +125,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_->add_shared(convert_ros_message_to_subscribed_type_unique_ptr(*message)); trigger_guard_condition(); } + invoke_on_new_message_listener(); } void @@ -137,6 +138,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message)); trigger_guard_condition(); } + invoke_on_new_message_listener(); } void @@ -144,6 +146,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff { buffer_->add_shared(std::move(message)); trigger_guard_condition(); + invoke_on_new_message_listener(); } void @@ -151,6 +154,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff { buffer_->add_unique(std::move(message)); trigger_guard_condition(); + invoke_on_new_message_listener(); } bool @@ -169,6 +173,18 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff BufferUniquePtr buffer_; SubscribedTypeAllocator subscribed_type_allocator_; SubscribedTypeDeleter subscribed_type_deleter_; + +private: + void + invoke_on_new_message_listener() + { + std::lock_guard lock(this->listener_mutex_); + if (this->on_new_message_callback_) { + this->on_new_message_callback_(1); + } else { + this->unread_count_++; + } + } }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/publisher_base.hpp b/rclcpp/include/rclcpp/publisher_base.hpp index 56292f1fbe..cdbd4bb758 100644 --- a/rclcpp/include/rclcpp/publisher_base.hpp +++ b/rclcpp/include/rclcpp/publisher_base.hpp @@ -282,7 +282,7 @@ class PublisherBase : public std::enable_shared_from_this if (event_handlers_.count(event_type) == 0) { RCLCPP_WARN( rclcpp::get_logger("rclcpp"), - "Calling set_on_new_qos_event_callback for non registered event_type"); + "Calling set_on_new_qos_event_callback for non registered publisher event_type"); return; } diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index b32e79c464..0c2963211c 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -178,7 +178,7 @@ class QOSEventHandlerBase : public Waitable } }; - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has @@ -200,7 +200,7 @@ class QOSEventHandlerBase : public Waitable void clear_on_ready_callback() override { - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); if (on_new_event_callback_) { set_on_new_event_callback(nullptr, nullptr); on_new_event_callback_ = nullptr; @@ -214,7 +214,7 @@ class QOSEventHandlerBase : public Waitable rcl_event_t event_handle_; size_t wait_set_event_index_; - std::recursive_mutex reentrant_mutex_; + std::recursive_mutex listener_mutex_; std::function on_new_event_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index 99680297ed..c4a82b86dc 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -182,7 +182,7 @@ class ServiceBase } }; - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has @@ -204,7 +204,7 @@ class ServiceBase void clear_on_new_request_callback() { - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); if (on_new_request_callback_) { set_on_new_request_callback(nullptr, nullptr); on_new_request_callback_ = nullptr; @@ -235,7 +235,7 @@ class ServiceBase std::atomic in_use_by_wait_set_{false}; - std::recursive_mutex reentrant_mutex_; + std::recursive_mutex listener_mutex_; std::function on_new_request_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 2d52388e3b..69b6031405 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -197,6 +197,14 @@ class Subscription : public SubscriptionBase "intraprocess communication allowed only with volatile durability"); } + using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< + MessageT, + SubscribedType, + SubscribedTypeAllocator, + SubscribedTypeDeleter, + ROSMessageT, + AllocatorT>; + // First create a SubscriptionIntraProcess which will be given to the intra-process manager. auto context = node_base->get_context(); subscription_intra_process_ = std::make_shared( @@ -404,15 +412,6 @@ class Subscription : public SubscriptionBase /// Component which computes and publishes topic statistics for this subscriber SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics_{nullptr}; - - using SubscriptionIntraProcessT = rclcpp::experimental::SubscriptionIntraProcess< - MessageT, - SubscribedType, - SubscribedTypeAllocator, - SubscribedTypeDeleter, - ROSMessageT, - AllocatorT>; - std::shared_ptr subscription_intra_process_; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index a464694cee..4585247cac 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -340,7 +340,7 @@ class SubscriptionBase : public std::enable_shared_from_this } }; - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has @@ -362,7 +362,7 @@ class SubscriptionBase : public std::enable_shared_from_this void clear_on_new_message_callback() { - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); if (on_new_message_callback_) { set_on_new_message_callback(nullptr, nullptr); @@ -459,7 +459,7 @@ class SubscriptionBase : public std::enable_shared_from_this if (event_handlers_.count(event_type) == 0) { RCLCPP_WARN( rclcpp::get_logger("rclcpp"), - "Calling set_on_new_qos_event_callback for non registered event_type"); + "Calling set_on_new_qos_event_callback for non registered subscription event_type"); return; } @@ -544,7 +544,7 @@ class SubscriptionBase : public std::enable_shared_from_this std::unordered_map> qos_events_in_use_by_wait_set_; - std::recursive_mutex reentrant_mutex_; + std::recursive_mutex listener_mutex_; std::function on_new_message_callback_{nullptr}; }; diff --git a/rclcpp/src/rclcpp/publisher_base.cpp b/rclcpp/src/rclcpp/publisher_base.cpp index 55d7897214..dd027f2e65 100644 --- a/rclcpp/src/rclcpp/publisher_base.cpp +++ b/rclcpp/src/rclcpp/publisher_base.cpp @@ -100,7 +100,7 @@ PublisherBase::PublisherBase( PublisherBase::~PublisherBase() { - for (const auto & pair : event_handlers_ ) { + for (const auto & pair : event_handlers_) { rcl_publisher_event_type_t event_type = pair.first; clear_on_new_qos_event_callback(event_type); } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index b85ae34b1e..12841fe6b6 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -87,7 +87,7 @@ SubscriptionBase::~SubscriptionBase() { clear_on_new_message_callback(); - for (const auto & pair : event_handlers_ ) { + for (const auto & pair : event_handlers_) { rcl_subscription_event_type_t event_type = pair.first; clear_on_new_qos_event_callback(event_type); } diff --git a/rclcpp/test/rclcpp/test_qos_event.cpp b/rclcpp/test/rclcpp/test_qos_event.cpp index bd6a0e77ed..d7ab0775f5 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -330,66 +330,43 @@ TEST_F(TestQosEvent, add_to_wait_set) { TEST_F(TestQosEvent, test_on_new_event_callback) { + auto offered_deadline = rclcpp::Duration(std::chrono::milliseconds(1)); + auto requested_deadline = rclcpp::Duration(std::chrono::milliseconds(2)); + rclcpp::QoS qos_profile_publisher(10); - qos_profile_publisher.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + qos_profile_publisher.deadline(offered_deadline); + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.deadline_callback = [](auto) {FAIL();}; auto publisher = node->create_publisher( - topic_name, qos_profile_publisher); + topic_name, qos_profile_publisher, pub_options); + + rclcpp::QoS qos_profile_subscription(10); + qos_profile_subscription.deadline(requested_deadline); + rclcpp::SubscriptionOptions sub_options; + sub_options.event_callbacks.deadline_callback = [](auto) {FAIL();}; + auto subscription = node->create_subscription( + topic_name, qos_profile_subscription, message_callback, sub_options); std::atomic c1 {0}; auto increase_c1_cb = [&c1](size_t count_events) {c1 += count_events;}; - publisher->set_on_new_qos_event_callback(increase_c1_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + publisher->set_on_new_qos_event_callback(increase_c1_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); - rclcpp::QoS qos_profile_subscription(10); - qos_profile_subscription.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - auto subscription = node->create_subscription( - topic_name, qos_profile_subscription, message_callback); + { + test_msgs::msg::Empty msg; + publisher->publish(msg); + } - auto start = std::chrono::steady_clock::now(); - do { - std::this_thread::sleep_for(100ms); - } while (c1 == 0 && std::chrono::steady_clock::now() - start < 10s); + std::this_thread::sleep_for(std::chrono::seconds(1)); - EXPECT_EQ(c1, 1u); + EXPECT_GT(c1, 1u); std::atomic c2 {0}; auto increase_c2_cb = [&c2](size_t count_events) {c2 += count_events;}; subscription->set_on_new_qos_event_callback( increase_c2_cb, - RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); - - start = std::chrono::steady_clock::now(); - do { - std::this_thread::sleep_for(100ms); - } while (c2 == 0 && std::chrono::steady_clock::now() - start < 10s); - - EXPECT_EQ(c1, 1u); - EXPECT_EQ(c2, 1u); - - auto publisher2 = node->create_publisher( - topic_name, qos_profile_publisher); - - start = std::chrono::steady_clock::now(); - do { - std::this_thread::sleep_for(100ms); - } while (c2 == 1u && std::chrono::steady_clock::now() - start < 10s); - - EXPECT_EQ(c1, 1u); - EXPECT_EQ(c2, 2u); - - publisher->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); - - auto subscription2 = node->create_subscription( - topic_name, qos_profile_subscription, message_callback); - - publisher->set_on_new_qos_event_callback(increase_c1_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS); + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); - start = std::chrono::steady_clock::now(); - do { - std::this_thread::sleep_for(100ms); - } while (c1 == 1 && std::chrono::steady_clock::now() - start < 10s); - - EXPECT_EQ(c1, 2u); - EXPECT_EQ(c2, 2u); + EXPECT_GT(c2, 1u); } TEST_F(TestQosEvent, test_invalid_on_new_event_callback) @@ -397,7 +374,18 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) auto pub = node->create_publisher(topic_name, 10); auto sub = node->create_subscription(topic_name, 10, message_callback); auto dummy_cb = [](size_t count_events) {(void)count_events;}; - std::function invalid_cb; + + EXPECT_NO_THROW( + pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED)); + + EXPECT_NO_THROW( + pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_DEADLINE_MISSED)); + + EXPECT_NO_THROW( + pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_LIVELINESS_LOST)); + + EXPECT_NO_THROW( + pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_LIVELINESS_LOST)); EXPECT_NO_THROW( pub->set_on_new_qos_event_callback(dummy_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS)); @@ -405,17 +393,40 @@ TEST_F(TestQosEvent, test_invalid_on_new_event_callback) EXPECT_NO_THROW( pub->clear_on_new_qos_event_callback(RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS)); + EXPECT_NO_THROW( + sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED)); + + EXPECT_NO_THROW( + sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED)); + + EXPECT_NO_THROW( + sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_LIVELINESS_CHANGED)); + + EXPECT_NO_THROW( + sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_LIVELINESS_CHANGED)); + EXPECT_NO_THROW( sub->set_on_new_qos_event_callback(dummy_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS)); EXPECT_NO_THROW( sub->clear_on_new_qos_event_callback(RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS)); + std::function invalid_cb; + + rclcpp::SubscriptionOptions sub_options; + sub_options.event_callbacks.deadline_callback = [](auto) {}; + sub = node->create_subscription( + topic_name, 10, message_callback, sub_options); + EXPECT_THROW( - pub->set_on_new_qos_event_callback(invalid_cb, RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS), + sub->set_on_new_qos_event_callback(invalid_cb, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED), std::invalid_argument); + rclcpp::PublisherOptions pub_options; + pub_options.event_callbacks.deadline_callback = [](auto) {}; + pub = node->create_publisher(topic_name, 10, pub_options); + EXPECT_THROW( - sub->set_on_new_qos_event_callback(invalid_cb, RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS), + pub->set_on_new_qos_event_callback(invalid_cb, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED), std::invalid_argument); } diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index daa8b89b0d..68efc6c817 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -298,7 +298,7 @@ class ClientBase : public rclcpp::Waitable const void * user_data); // Mutex to protect the callbacks storage. - std::recursive_mutex reentrant_mutex_; + std::recursive_mutex listener_mutex_; // Storage for std::function callbacks to keep them in scope std::unordered_map> entity_type_to_on_ready_callback_; diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 7861293d28..336c5b5dbf 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -307,7 +307,7 @@ class ServerBase : public rclcpp::Waitable protected: // Mutex to protect the callbacks storage. - std::recursive_mutex reentrant_mutex_; + std::recursive_mutex listener_mutex_; // Storage for std::function callbacks to keep them in scope std::unordered_map> entity_type_to_on_ready_callback_; diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 65e42bb9e7..0bd87963c2 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -437,7 +437,7 @@ ClientBase::set_callback_to_entity( rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); // Store the std::function to keep it in scope, also overwrites the existing one. auto it = entity_type_to_on_ready_callback_.find(entity_type); @@ -533,7 +533,7 @@ ClientBase::clear_on_ready_callback() set_on_ready_callback(EntityType::FeedbackSubscription, nullptr, nullptr); set_on_ready_callback(EntityType::StatusSubscription, nullptr, nullptr); - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); entity_type_to_on_ready_callback_.clear(); } diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 23154e3b48..bec903839e 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -729,7 +729,7 @@ ServerBase::set_callback_to_entity( rclcpp::detail::cpp_callback_trampoline, static_cast(&new_callback)); - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); // Store the std::function to keep it in scope, also overwrites the existing one. auto it = entity_type_to_on_ready_callback_.find(entity_type); @@ -805,6 +805,6 @@ ServerBase::clear_on_ready_callback() set_on_ready_callback(EntityType::ResultService, nullptr, nullptr); set_on_ready_callback(EntityType::CancelService, nullptr, nullptr); - std::lock_guard lock(reentrant_mutex_); + std::lock_guard lock(listener_mutex_); entity_type_to_on_ready_callback_.clear(); } From 5af4cb28326fb0b01003b6a388782e4205f58765 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 9 Feb 2022 12:26:25 -0300 Subject: [PATCH 22/31] Add GuardCondition on trigger callback Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/guard_condition.hpp | 16 ++++++++ rclcpp/src/rclcpp/guard_condition.cpp | 48 +++++++++++++++++++++-- 2 files changed, 61 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index fce42f1d4b..f614294505 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -98,10 +98,26 @@ class GuardCondition bool exchange_in_use_by_wait_set_state(bool in_use_state); + /// Adds the guard condition to a waitset + /** + * This function is thread-safe. + * \param[in] wait_set pointer to a wait set where to add the guard condition + */ + RCLCPP_PUBLIC + void + add_to_wait_set(rcl_wait_set_t * wait_set); + + RCLCPP_PUBLIC + void + set_on_trigger_callback(std::function callback); + protected: rclcpp::Context::SharedPtr context_; rcl_guard_condition_t rcl_guard_condition_; std::atomic in_use_by_wait_set_{false}; + std::recursive_mutex reentrant_mutex_; + std::function on_trigger_callback_{nullptr}; + size_t unread_count_{0}; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index cae0c4ce5c..8efd541902 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/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 + #include "rclcpp/guard_condition.hpp" #include "rclcpp/exceptions.hpp" @@ -72,9 +74,16 @@ 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); + std::lock_guard lock(reentrant_mutex_); + + 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_++; } } @@ -84,4 +93,37 @@ GuardCondition::exchange_in_use_by_wait_set_state(bool in_use_state) return in_use_by_wait_set_.exchange(in_use_state); } +void +GuardCondition::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + if (exchange_in_use_by_wait_set_state(true)) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "guard condition already added to a wait set"); + } + rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &this->rcl_guard_condition_, NULL); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to add guard condition to wait set"); + } +} + +void +GuardCondition::set_on_trigger_callback(std::function callback) +{ + std::lock_guard lock(reentrant_mutex_); + + if (callback) { + on_trigger_callback_ = callback; + + if (unread_count_) { + callback(unread_count_); + unread_count_ = 0; + } + return; + } + + on_trigger_callback_ = nullptr; +} + } // namespace rclcpp From 9d0ac6c46cea7bbd67e1f36f8e5df5c89e468f6b Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Wed, 9 Feb 2022 15:40:10 -0300 Subject: [PATCH 23/31] Add tests for new GuardCondition APIs Signed-off-by: Mauro Passerino --- rclcpp/include/rclcpp/guard_condition.hpp | 1 + rclcpp/src/rclcpp/guard_condition.cpp | 11 +++- rclcpp/test/rclcpp/test_guard_condition.cpp | 62 +++++++++++++++++++++ 3 files changed, 71 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index f614294505..f6f5af9586 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -118,6 +118,7 @@ class GuardCondition std::recursive_mutex reentrant_mutex_; std::function on_trigger_callback_{nullptr}; size_t unread_count_{0}; + rcl_wait_set_t * wait_set_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index 8efd541902..ea68c78d73 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -96,11 +96,16 @@ GuardCondition::exchange_in_use_by_wait_set_state(bool in_use_state) void GuardCondition::add_to_wait_set(rcl_wait_set_t * wait_set) { + std::lock_guard lock(reentrant_mutex_); + if (exchange_in_use_by_wait_set_state(true)) { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "guard condition already added to a wait set"); + if (wait_set != wait_set_) { + throw std::runtime_error("guard condition has already been added to a wait set."); + } + } else { + wait_set_ = wait_set; } + rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &this->rcl_guard_condition_, NULL); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( diff --git a/rclcpp/test/rclcpp/test_guard_condition.cpp b/rclcpp/test/rclcpp/test_guard_condition.cpp index 8100cf1c9b..481051ccf9 100644 --- a/rclcpp/test/rclcpp/test_guard_condition.cpp +++ b/rclcpp/test/rclcpp/test_guard_condition.cpp @@ -102,3 +102,65 @@ TEST_F(TestGuardCondition, trigger) { } } } + +/* + * Testing addition to a wait set + */ +TEST_F(TestGuardCondition, add_to_wait_set) { + { + { + auto gc = std::make_shared(); + + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_OK); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + EXPECT_NO_THROW(gc->add_to_wait_set(&wait_set)); + EXPECT_NO_THROW(gc->add_to_wait_set(&wait_set)); + + rcl_wait_set_t wait_set_2 = rcl_get_zero_initialized_wait_set(); + EXPECT_THROW(gc->add_to_wait_set(&wait_set_2), std::runtime_error); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_wait_set_add_guard_condition, RCL_RET_ERROR); + + auto gd = std::make_shared(); + EXPECT_THROW(gd->add_to_wait_set(nullptr), rclcpp::exceptions::RCLError); + } + } +} + +/* + * Testing set on trigger callback + */ +TEST_F(TestGuardCondition, set_on_trigger_callback) { + { + 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); + + EXPECT_EQ(c1.load(), 0u); + EXPECT_NO_THROW(gc->trigger()); + EXPECT_EQ(c1.load(), 1u); + + std::atomic c2 {0}; + auto increase_c2_cb = [&c2](size_t count_msgs) {c2 += count_msgs;}; + gc->set_on_trigger_callback(increase_c2_cb); + + EXPECT_NO_THROW(gc->trigger()); + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + gc->set_on_trigger_callback(nullptr); + EXPECT_NO_THROW(gc->trigger()); + EXPECT_EQ(c1.load(), 1u); + EXPECT_EQ(c2.load(), 1u); + + gc->set_on_trigger_callback(increase_c1_cb); + EXPECT_EQ(c1.load(), 2u); + } +} From acae8860f5b629a016da31f9e2b5583627b73644 Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Thu, 10 Feb 2022 13:42:54 -0300 Subject: [PATCH 24/31] Fix windows CI Signed-off-by: Mauro Passerino --- .../rclcpp/experimental/subscription_intra_process_base.hpp | 1 + rclcpp/include/rclcpp/waitable.hpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 4469c92031..b3069a4880 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -51,6 +51,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable : gc_(context), topic_name_(topic_name), qos_profile_(qos_profile) {} + RCLCPP_PUBLIC virtual ~SubscriptionIntraProcessBase(); RCLCPP_PUBLIC diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 9ddc8a0882..db06f70678 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -225,6 +225,7 @@ class Waitable * * \param[in] callback functor to be called when the waitable becomes ready */ + RCLCPP_PUBLIC virtual void set_on_ready_callback(std::function callback); @@ -234,6 +235,7 @@ class Waitable * Note: this function must be overridden with a proper implementation * by the custom classes who inherit from rclcpp::Waitable if they want to use it. */ + RCLCPP_PUBLIC virtual void clear_on_ready_callback(); From 540e6bbc15146028ae7545fa86a3de03f54e07fa Mon Sep 17 00:00:00 2001 From: Mauro Passerino Date: Thu, 10 Feb 2022 17:12:12 -0300 Subject: [PATCH 25/31] Action unset callbacks only if were set Signed-off-by: Mauro Passerino --- rclcpp_action/include/rclcpp_action/client.hpp | 2 ++ rclcpp_action/include/rclcpp_action/server.hpp | 2 ++ rclcpp_action/src/client.cpp | 18 ++++++++++++------ rclcpp_action/src/server.cpp | 14 ++++++++++---- 4 files changed, 26 insertions(+), 10 deletions(-) diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 68efc6c817..872aaf693f 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -311,6 +311,8 @@ class ClientBase : public rclcpp::Waitable set_callback_to_entity( EntityType entity_type, std::function callback); + + bool on_ready_callback_set_{false}; }; /// Action Client diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 336c5b5dbf..efbd2081e8 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -318,6 +318,8 @@ class ServerBase : public rclcpp::Waitable EntityType entity_type, rcl_event_callback_t callback, const void * user_data); + + bool on_ready_callback_set_{false}; }; /// Action Server diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index 0bd87963c2..07982092d5 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -457,6 +457,8 @@ ClientBase::set_callback_to_entity( rclcpp::detail::cpp_callback_trampoline, static_cast(&cb)); } + + on_ready_callback_set_ = true; } void @@ -527,13 +529,17 @@ ClientBase::set_on_ready_callback( void ClientBase::clear_on_ready_callback() { - set_on_ready_callback(EntityType::GoalClient, nullptr, nullptr); - set_on_ready_callback(EntityType::ResultClient, nullptr, nullptr); - set_on_ready_callback(EntityType::CancelClient, nullptr, nullptr); - set_on_ready_callback(EntityType::FeedbackSubscription, nullptr, nullptr); - set_on_ready_callback(EntityType::StatusSubscription, nullptr, nullptr); - std::lock_guard lock(listener_mutex_); + + if (on_ready_callback_set_) { + set_on_ready_callback(EntityType::GoalClient, nullptr, nullptr); + set_on_ready_callback(EntityType::ResultClient, nullptr, nullptr); + set_on_ready_callback(EntityType::CancelClient, nullptr, nullptr); + set_on_ready_callback(EntityType::FeedbackSubscription, nullptr, nullptr); + set_on_ready_callback(EntityType::StatusSubscription, nullptr, nullptr); + on_ready_callback_set_ = false; + } + entity_type_to_on_ready_callback_.clear(); } diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index bec903839e..a07e203e28 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -749,6 +749,8 @@ ServerBase::set_callback_to_entity( rclcpp::detail::cpp_callback_trampoline, static_cast(&cb)); } + + on_ready_callback_set_ = true; } void @@ -801,10 +803,14 @@ ServerBase::set_on_ready_callback( void ServerBase::clear_on_ready_callback() { - set_on_ready_callback(EntityType::GoalService, nullptr, nullptr); - set_on_ready_callback(EntityType::ResultService, nullptr, nullptr); - set_on_ready_callback(EntityType::CancelService, nullptr, nullptr); - std::lock_guard lock(listener_mutex_); + + if (on_ready_callback_set_) { + set_on_ready_callback(EntityType::GoalService, nullptr, nullptr); + set_on_ready_callback(EntityType::ResultService, nullptr, nullptr); + set_on_ready_callback(EntityType::CancelService, nullptr, nullptr); + on_ready_callback_set_ = false; + } + entity_type_to_on_ready_callback_.clear(); } From e5937db9e13ea70dfbf7960f0b674916906155fc Mon Sep 17 00:00:00 2001 From: Alberto Soragna Date: Mon, 14 Feb 2022 20:21:13 +0000 Subject: [PATCH 26/31] add missing include rcl event callback include directive Signed-off-by: Alberto Soragna --- rclcpp/include/rclcpp/client.hpp | 1 + rclcpp/include/rclcpp/qos_event.hpp | 1 + rclcpp/include/rclcpp/service.hpp | 1 + rclcpp/include/rclcpp/subscription_base.hpp | 1 + rclcpp_action/include/rclcpp_action/client.hpp | 2 ++ rclcpp_action/include/rclcpp_action/server.hpp | 1 + 6 files changed, 7 insertions(+) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index b0da172559..a1deef73f6 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -30,6 +30,7 @@ #include "rcl/client.h" #include "rcl/error_handling.h" +#include "rcl/event_callback.h" #include "rcl/wait.h" #include "rclcpp/detail/cpp_callback_trampoline.hpp" diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 0c2963211c..95e95ba0b9 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -22,6 +22,7 @@ #include #include "rcl/error_handling.h" +#include "rcl/event_callback.h" #include "rmw/impl/cpp/demangle.hpp" #include "rmw/incompatible_qos_events_statuses.h" diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index c4a82b86dc..f1f8484436 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -24,6 +24,7 @@ #include #include "rcl/error_handling.h" +#include "rcl/event_callback.h" #include "rcl/service.h" #include "rmw/error_handling.h" diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 4585247cac..eff4b64052 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -23,6 +23,7 @@ #include #include +#include "rcl/event_callback.h" #include "rcl/subscription.h" #include "rmw/impl/cpp/demangle.hpp" diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index 872aaf693f..9bcddadec0 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -26,6 +26,8 @@ #include #include +#include "rcl/event_callback.h" + #include "rclcpp/exceptions.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index efbd2081e8..554cb1cf56 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -22,6 +22,7 @@ #include #include +#include "rcl/event_callback.h" #include "rcl_action/action_server.h" #include "rosidl_runtime_c/action_type_support_struct.h" #include "rosidl_typesupport_cpp/action_type_support.hpp" From df55696b52f92970c13feac6b1bc5ab2c69ce4e6 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 23 Feb 2022 01:19:37 -0800 Subject: [PATCH 27/31] typos Signed-off-by: William Woodall --- rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp | 2 +- .../rclcpp/experimental/subscription_intra_process_base.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp index 7ea991c0de..be857e1b58 100644 --- a/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp +++ b/rclcpp/include/rclcpp/detail/cpp_callback_trampoline.hpp @@ -38,7 +38,7 @@ namespace detail * captures, and various kinds of std::bind instances. * * The interior of this function is likely to be executed within a C runtime, - * so no exceptins should be thrown at this point, and doing so results in + * so no exceptions should be thrown at this point, and doing so results in * undefined behavior. * * \tparam UserDataT Deduced type based on what is passed for user data, diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index b3069a4880..a23d20bc81 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -96,7 +96,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable * The application should provide a generic callback function that will be then * forwarded by the waitable to all of its entities. * Before forwarding, a different value for the identifier argument will be - * bond to the function. + * bound to the function. * This implies that the provided callback can use the identifier to behave * differently depending on which entity triggered the waitable to become ready. * From 0b0659e22cc3a561c8fb9f4e19fc64c2d4af91ed Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 23 Feb 2022 01:19:55 -0800 Subject: [PATCH 28/31] remove listener reference Signed-off-by: William Woodall --- .../experimental/subscription_intra_process_buffer.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 3f7dc48fa6..11c964ad00 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -125,7 +125,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_->add_shared(convert_ros_message_to_subscribed_type_unique_ptr(*message)); trigger_guard_condition(); } - invoke_on_new_message_listener(); + invoke_on_new_message(); } void @@ -138,7 +138,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message)); trigger_guard_condition(); } - invoke_on_new_message_listener(); + invoke_on_new_message(); } void @@ -146,7 +146,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff { buffer_->add_shared(std::move(message)); trigger_guard_condition(); - invoke_on_new_message_listener(); + invoke_on_new_message(); } void @@ -154,7 +154,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff { buffer_->add_unique(std::move(message)); trigger_guard_condition(); - invoke_on_new_message_listener(); + invoke_on_new_message(); } bool @@ -176,7 +176,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff private: void - invoke_on_new_message_listener() + invoke_on_new_message() { std::lock_guard lock(this->listener_mutex_); if (this->on_new_message_callback_) { From 89b8cb3f3bdc6ec57270637a3b76c7eaca4c18eb Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 23 Feb 2022 02:24:52 -0800 Subject: [PATCH 29/31] remove references to listener and move a method closer to impl Signed-off-by: William Woodall --- rclcpp/include/rclcpp/client.hpp | 6 +++--- .../subscription_intra_process_base.hpp | 17 ++++++++++++++--- .../subscription_intra_process_buffer.hpp | 19 ++++--------------- rclcpp/include/rclcpp/qos_event.hpp | 6 +++--- rclcpp/include/rclcpp/service.hpp | 6 +++--- rclcpp/include/rclcpp/subscription_base.hpp | 6 +++--- 6 files changed, 30 insertions(+), 30 deletions(-) diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp index a1deef73f6..e169b890bb 100644 --- a/rclcpp/include/rclcpp/client.hpp +++ b/rclcpp/include/rclcpp/client.hpp @@ -273,7 +273,7 @@ class ClientBase } }; - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has @@ -295,7 +295,7 @@ class ClientBase void clear_on_new_response_callback() { - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); if (on_new_response_callback_) { set_on_new_response_callback(nullptr, nullptr); on_new_response_callback_ = nullptr; @@ -330,7 +330,7 @@ class ClientBase std::atomic in_use_by_wait_set_{false}; - std::recursive_mutex listener_mutex_; + std::recursive_mutex callback_mutex_; std::function on_new_response_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index a23d20bc81..57ed36b445 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -142,7 +142,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable } }; - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); on_new_message_callback_ = new_callback; if (unread_count_ > 0) { @@ -160,12 +160,12 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable void clear_on_ready_callback() override { - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); on_new_message_callback_ = nullptr; } protected: - std::recursive_mutex listener_mutex_; + std::recursive_mutex callback_mutex_; std::function on_new_message_callback_ {nullptr}; size_t unread_count_{0}; rclcpp::GuardCondition gc_; @@ -173,6 +173,17 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable virtual void trigger_guard_condition() = 0; + void + invoke_on_new_message() + { + std::lock_guard lock(this->callback_mutex_); + if (this->on_new_message_callback_) { + this->on_new_message_callback_(1); + } else { + this->unread_count_++; + } + } + private: std::string topic_name_; QoS qos_profile_; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index 11c964ad00..d8b7d3e0cd 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -125,7 +125,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_->add_shared(convert_ros_message_to_subscribed_type_unique_ptr(*message)); trigger_guard_condition(); } - invoke_on_new_message(); + this->invoke_on_new_message(); } void @@ -138,7 +138,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff buffer_->add_unique(convert_ros_message_to_subscribed_type_unique_ptr(*message)); trigger_guard_condition(); } - invoke_on_new_message(); + this->invoke_on_new_message(); } void @@ -146,7 +146,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff { buffer_->add_shared(std::move(message)); trigger_guard_condition(); - invoke_on_new_message(); + this->invoke_on_new_message(); } void @@ -154,7 +154,7 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff { buffer_->add_unique(std::move(message)); trigger_guard_condition(); - invoke_on_new_message(); + this->invoke_on_new_message(); } bool @@ -174,17 +174,6 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff SubscribedTypeAllocator subscribed_type_allocator_; SubscribedTypeDeleter subscribed_type_deleter_; -private: - void - invoke_on_new_message() - { - std::lock_guard lock(this->listener_mutex_); - if (this->on_new_message_callback_) { - this->on_new_message_callback_(1); - } else { - this->unread_count_++; - } - } }; } // namespace experimental diff --git a/rclcpp/include/rclcpp/qos_event.hpp b/rclcpp/include/rclcpp/qos_event.hpp index 95e95ba0b9..59c99dda42 100644 --- a/rclcpp/include/rclcpp/qos_event.hpp +++ b/rclcpp/include/rclcpp/qos_event.hpp @@ -179,7 +179,7 @@ class QOSEventHandlerBase : public Waitable } }; - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has @@ -201,7 +201,7 @@ class QOSEventHandlerBase : public Waitable void clear_on_ready_callback() override { - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); if (on_new_event_callback_) { set_on_new_event_callback(nullptr, nullptr); on_new_event_callback_ = nullptr; @@ -215,7 +215,7 @@ class QOSEventHandlerBase : public Waitable rcl_event_t event_handle_; size_t wait_set_event_index_; - std::recursive_mutex listener_mutex_; + std::recursive_mutex callback_mutex_; std::function on_new_event_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp index f1f8484436..47704a4c09 100644 --- a/rclcpp/include/rclcpp/service.hpp +++ b/rclcpp/include/rclcpp/service.hpp @@ -183,7 +183,7 @@ class ServiceBase } }; - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has @@ -205,7 +205,7 @@ class ServiceBase void clear_on_new_request_callback() { - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); if (on_new_request_callback_) { set_on_new_request_callback(nullptr, nullptr); on_new_request_callback_ = nullptr; @@ -236,7 +236,7 @@ class ServiceBase std::atomic in_use_by_wait_set_{false}; - std::recursive_mutex listener_mutex_; + std::recursive_mutex callback_mutex_; std::function on_new_request_callback_{nullptr}; }; diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index eff4b64052..26650cd121 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -341,7 +341,7 @@ class SubscriptionBase : public std::enable_shared_from_this } }; - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); // Set it temporarily to the new callback, while we replace the old one. // This two-step setting, prevents a gap where the old std::function has @@ -363,7 +363,7 @@ class SubscriptionBase : public std::enable_shared_from_this void clear_on_new_message_callback() { - std::lock_guard lock(listener_mutex_); + std::lock_guard lock(callback_mutex_); if (on_new_message_callback_) { set_on_new_message_callback(nullptr, nullptr); @@ -545,7 +545,7 @@ class SubscriptionBase : public std::enable_shared_from_this std::unordered_map> qos_events_in_use_by_wait_set_; - std::recursive_mutex listener_mutex_; + std::recursive_mutex callback_mutex_; std::function on_new_message_callback_{nullptr}; }; From d9309020ca33afa1cf0a63faa068a22556c05443 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 23 Feb 2022 08:00:31 -0800 Subject: [PATCH 30/31] cpplint Signed-off-by: William Woodall --- .../rclcpp/experimental/subscription_intra_process_buffer.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index d8b7d3e0cd..3c71512677 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -173,7 +173,6 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff BufferUniquePtr buffer_; SubscribedTypeAllocator subscribed_type_allocator_; SubscribedTypeDeleter subscribed_type_deleter_; - }; } // namespace experimental From f51a1be7a554ab9ff7ff173d46fb398303a5213a Mon Sep 17 00:00:00 2001 From: iRobot ROS <49500531+irobot-ros@users.noreply.github.com> Date: Wed, 23 Feb 2022 16:55:36 +0000 Subject: [PATCH 31/31] Fix qos history check in subscription_intra_process.hpp --- .../rclcpp/experimental/subscription_intra_process_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 57ed36b445..331c1b6da5 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp @@ -146,7 +146,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable on_new_message_callback_ = new_callback; if (unread_count_ > 0) { - if (qos_profile_.history() == HistoryPolicy::KeepLast || qos_profile_.depth() == 0) { + if (qos_profile_.history() == HistoryPolicy::KeepAll) { on_new_message_callback_(unread_count_); } else { // Use qos profile depth as upper bound for unread_count_