From 5242d5b682e5703e44502361cbad4c77d931acff Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Tue, 2 Apr 2024 00:16:32 -0400 Subject: [PATCH] Revert "Revert "update rclcpp::Waitable API to use references and const (#2467)"" This reverts commit 05cb18277eedeb308e0a1996ce069635a0300782. --- rclcpp/include/rclcpp/event_handler.hpp | 6 +- .../executors/executor_notify_waitable.hpp | 6 +- .../subscription_intra_process.hpp | 7 +- .../subscription_intra_process_base.hpp | 6 +- .../subscription_intra_process_buffer.hpp | 6 +- rclcpp/include/rclcpp/guard_condition.hpp | 6 +- .../strategies/allocator_memory_strategy.hpp | 4 +- rclcpp/include/rclcpp/wait_result.hpp | 2 +- .../detail/storage_policy_common.hpp | 2 +- rclcpp/include/rclcpp/waitable.hpp | 57 ++++++++++++- rclcpp/src/rclcpp/event_handler.cpp | 8 +- rclcpp/src/rclcpp/executor.cpp | 3 +- .../executor_entities_collection.cpp | 2 +- .../executors/executor_notify_waitable.cpp | 13 ++- .../static_single_threaded_executor.cpp | 2 +- .../events_executor/events_executor.cpp | 2 +- rclcpp/src/rclcpp/guard_condition.cpp | 8 +- .../subscription_intra_process_base.cpp | 4 +- rclcpp/src/rclcpp/waitable.cpp | 79 +++++++++++++++++++ .../test/rclcpp/executors/test_executors.cpp | 13 ++- ...est_static_executor_entities_collector.cpp | 14 +--- .../node_interfaces/test_node_waitables.cpp | 9 +-- .../test_allocator_memory_strategy.cpp | 17 ++-- rclcpp/test/rclcpp/test_guard_condition.cpp | 14 +--- rclcpp/test/rclcpp/test_memory_strategy.cpp | 6 +- rclcpp/test/rclcpp/test_publisher.cpp | 2 +- rclcpp/test/rclcpp/test_qos_event.cpp | 4 +- .../test_dynamic_storage.cpp | 6 +- .../wait_set_policies/test_static_storage.cpp | 6 +- .../test_storage_policy_common.cpp | 6 +- .../test_thread_safe_synchronization.cpp | 6 +- .../include/rclcpp_action/client.hpp | 6 +- .../include/rclcpp_action/server.hpp | 12 +-- rclcpp_action/src/client.cpp | 11 +-- rclcpp_action/src/server.cpp | 19 ++--- rclcpp_action/test/test_client.cpp | 6 +- rclcpp_action/test/test_server.cpp | 6 +- 37 files changed, 246 insertions(+), 140 deletions(-) diff --git a/rclcpp/include/rclcpp/event_handler.hpp b/rclcpp/include/rclcpp/event_handler.hpp index f9b75eb7cf..887c571d16 100644 --- a/rclcpp/include/rclcpp/event_handler.hpp +++ b/rclcpp/include/rclcpp/event_handler.hpp @@ -117,12 +117,12 @@ class EventHandlerBase : public Waitable /// Add the Waitable to a wait set. RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + add_to_wait_set(rcl_wait_set_t & wait_set) override; /// Check if the Waitable is ready. RCLCPP_PUBLIC bool - is_ready(rcl_wait_set_t * wait_set) override; + is_ready(const rcl_wait_set_t & wait_set) override; /// Set a callback to be called when each new event instance occurs. /** @@ -294,7 +294,7 @@ class EventHandler : public EventHandlerBase /// Execute any entities of the Waitable that are ready. void - execute(std::shared_ptr & data) override + execute(const std::shared_ptr & data) override { if (!data) { throw std::runtime_error("'data' is empty"); diff --git a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp index 2b43fecca1..0de01fd4b0 100644 --- a/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp +++ b/rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp @@ -60,7 +60,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + add_to_wait_set(rcl_wait_set_t & wait_set) override; /// Check conditions against the wait set /** @@ -69,7 +69,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC bool - is_ready(rcl_wait_set_t * wait_set) override; + is_ready(const rcl_wait_set_t & wait_set) override; /// Perform work associated with the waitable. /** @@ -78,7 +78,7 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable */ RCLCPP_PUBLIC void - execute(std::shared_ptr & data) override; + execute(const std::shared_ptr & data) override; /// Retrieve data to be used in the next execute call. /** diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp index 39c391a027..5c562a61ff 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp @@ -132,7 +132,7 @@ class SubscriptionIntraProcess ); } - void execute(std::shared_ptr & data) override + void execute(const std::shared_ptr & data) override { execute_impl(data); } @@ -140,15 +140,14 @@ class SubscriptionIntraProcess protected: template typename std::enable_if::value, void>::type - execute_impl(std::shared_ptr & data) + execute_impl(const std::shared_ptr &) { - (void)data; throw std::runtime_error("Subscription intra-process can't handle serialized messages"); } template typename std::enable_if::value, void>::type - execute_impl(std::shared_ptr & data) + execute_impl(const std::shared_ptr & data) { if (!data) { return; diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp index 6a14aac0c7..74792e8751 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 RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + add_to_wait_set(rcl_wait_set_t & wait_set) override; RCLCPP_PUBLIC virtual @@ -72,7 +72,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable is_durability_transient_local() const; bool - is_ready(rcl_wait_set_t * wait_set) override = 0; + is_ready(const rcl_wait_set_t & wait_set) override = 0; std::shared_ptr take_data() override = 0; @@ -85,7 +85,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable } void - execute(std::shared_ptr & data) override = 0; + execute(const std::shared_ptr & data) override = 0; virtual bool diff --git a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp index abe5142581..2f384f351c 100644 --- a/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp +++ b/rclcpp/include/rclcpp/experimental/subscription_intra_process_buffer.hpp @@ -101,16 +101,16 @@ class SubscriptionIntraProcessBuffer : public SubscriptionROSMsgIntraProcessBuff } void - add_to_wait_set(rcl_wait_set_t * wait_set) override + add_to_wait_set(rcl_wait_set_t & wait_set) override { if (this->buffer_->has_data()) { this->trigger_guard_condition(); } - detail::add_guard_condition_to_rcl_wait_set(*wait_set, this->gc_); + detail::add_guard_condition_to_rcl_wait_set(wait_set, this->gc_); } bool - is_ready(rcl_wait_set_t * wait_set) override + is_ready(const rcl_wait_set_t & wait_set) override { (void) wait_set; return buffer_->has_data(); diff --git a/rclcpp/include/rclcpp/guard_condition.hpp b/rclcpp/include/rclcpp/guard_condition.hpp index 0a57bace22..594234657c 100644 --- a/rclcpp/include/rclcpp/guard_condition.hpp +++ b/rclcpp/include/rclcpp/guard_condition.hpp @@ -100,7 +100,7 @@ class GuardCondition */ RCLCPP_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set); + add_to_wait_set(rcl_wait_set_t & wait_set); /// Set a callback to be called whenever the guard condition is triggered. /** @@ -128,7 +128,9 @@ 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}; + // the type of wait_set_ is actually rcl_wait_set_t *, but it's never + // dereferenced, only compared to, so make it void * to avoid accidental use + void * wait_set_{nullptr}; }; } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp index 46379744a1..28e929c495 100644 --- a/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp +++ b/rclcpp/include/rclcpp/strategies/allocator_memory_strategy.hpp @@ -121,7 +121,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } } for (size_t i = 0; i < waitable_handles_.size(); ++i) { - if (waitable_handles_[i]->is_ready(wait_set)) { + if (waitable_handles_[i]->is_ready(*wait_set)) { waitable_triggered_handles_.emplace_back(std::move(waitable_handles_[i])); } } @@ -235,7 +235,7 @@ class AllocatorMemoryStrategy : public memory_strategy::MemoryStrategy } for (const std::shared_ptr & waitable : waitable_handles_) { - waitable->add_to_wait_set(wait_set); + waitable->add_to_wait_set(*wait_set); } return true; } diff --git a/rclcpp/include/rclcpp/wait_result.hpp b/rclcpp/include/rclcpp/wait_result.hpp index 429eb1dd25..74d3d2183c 100644 --- a/rclcpp/include/rclcpp/wait_result.hpp +++ b/rclcpp/include/rclcpp/wait_result.hpp @@ -277,7 +277,7 @@ class WaitResult final auto rcl_wait_set = wait_set.get_rcl_wait_set(); while (next_waitable_index_ < wait_set.size_of_waitables()) { auto cur_waitable = wait_set.waitables(next_waitable_index_++); - if (cur_waitable != nullptr && cur_waitable->is_ready(&rcl_wait_set)) { + if (cur_waitable != nullptr && cur_waitable->is_ready(rcl_wait_set)) { waitable = cur_waitable; break; } diff --git a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp index 42293a3e89..99050dfeba 100644 --- a/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp +++ b/rclcpp/include/rclcpp/wait_set_policies/detail/storage_policy_common.hpp @@ -375,7 +375,7 @@ class StoragePolicyCommon needs_pruning_ = true; continue; } - waitable_entry.waitable->add_to_wait_set(&rcl_wait_set_); + waitable_entry.waitable->add_to_wait_set(rcl_wait_set_); } } diff --git a/rclcpp/include/rclcpp/waitable.hpp b/rclcpp/include/rclcpp/waitable.hpp index 1a0d8b61f1..e834765a08 100644 --- a/rclcpp/include/rclcpp/waitable.hpp +++ b/rclcpp/include/rclcpp/waitable.hpp @@ -101,6 +101,23 @@ class Waitable size_t get_number_of_ready_guard_conditions(); +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Woverloaded-virtual" +#endif + /// Deprecated. + /** + * \sa add_to_wait_set(rcl_wait_set_t &) + */ + [[deprecated("Use add_to_wait_set(rcl_wait_set_t & wait_set) signature")]] + RCLCPP_PUBLIC + virtual + void + add_to_wait_set(rcl_wait_set_t * wait_set); +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + /// Add the Waitable to a wait set. /** * \param[in] wait_set A handle to the wait set to add the Waitable to. @@ -109,7 +126,24 @@ class Waitable RCLCPP_PUBLIC virtual void - add_to_wait_set(rcl_wait_set_t * wait_set) = 0; + add_to_wait_set(rcl_wait_set_t & wait_set); + +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Woverloaded-virtual" +#endif + /// Deprecated. + /** + * \sa is_ready(const rcl_wait_set_t &) + */ + [[deprecated("Use is_ready(const rcl_wait_set_t & wait_set) signature")]] + RCLCPP_PUBLIC + virtual + bool + is_ready(rcl_wait_set_t * wait_set); +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif /// Check if the Waitable is ready. /** @@ -124,7 +158,7 @@ class Waitable RCLCPP_PUBLIC virtual bool - is_ready(rcl_wait_set_t * wait_set) = 0; + is_ready(const rcl_wait_set_t & wait_set); /// Take the data so that it can be consumed with `execute`. /** @@ -178,6 +212,23 @@ class Waitable std::shared_ptr take_data_by_entity_id(size_t id); +#ifdef __GNUC__ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Woverloaded-virtual" +#endif + /// Deprecated. + /** + * \sa execute(const std::shared_ptr &) + */ + [[deprecated("Use execute(const std::shared_ptr & data) signature")]] + RCLCPP_PUBLIC + virtual + void + execute(std::shared_ptr & data); +#ifdef __GNUC__ +#pragma GCC diagnostic pop +#endif + /// Execute data that is passed in. /** * Before calling this method, the Waitable should be added to a wait set, @@ -203,7 +254,7 @@ class Waitable RCLCPP_PUBLIC virtual void - execute(std::shared_ptr & data) = 0; + execute(const std::shared_ptr & data); /// Exchange the "in use by wait set" state for this timer. /** diff --git a/rclcpp/src/rclcpp/event_handler.cpp b/rclcpp/src/rclcpp/event_handler.cpp index d4b4d57b08..6232e5b0d9 100644 --- a/rclcpp/src/rclcpp/event_handler.cpp +++ b/rclcpp/src/rclcpp/event_handler.cpp @@ -56,9 +56,9 @@ EventHandlerBase::get_number_of_ready_events() /// Add the Waitable to a wait set. void -EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +EventHandlerBase::add_to_wait_set(rcl_wait_set_t & wait_set) { - rcl_ret_t ret = rcl_wait_set_add_event(wait_set, &event_handle_, &wait_set_event_index_); + rcl_ret_t ret = rcl_wait_set_add_event(&wait_set, &event_handle_, &wait_set_event_index_); if (RCL_RET_OK != ret) { exceptions::throw_from_rcl_error(ret, "Couldn't add event to wait set"); } @@ -66,9 +66,9 @@ EventHandlerBase::add_to_wait_set(rcl_wait_set_t * wait_set) /// Check if the Waitable is ready. bool -EventHandlerBase::is_ready(rcl_wait_set_t * wait_set) +EventHandlerBase::is_ready(const rcl_wait_set_t & wait_set) { - return wait_set->events[wait_set_event_index_] == &event_handle_; + return wait_set.events[wait_set_event_index_] == &event_handle_; } void diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 776e6de4ea..a54d71e21b 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -398,7 +398,8 @@ Executor::execute_any_executable(AnyExecutable & any_exec) execute_client(any_exec.client); } if (any_exec.waitable) { - any_exec.waitable->execute(any_exec.data); + const std::shared_ptr & const_data = any_exec.data; + any_exec.waitable->execute(const_data); } // Reset the callback_group, regardless of type diff --git a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp index b6e030d340..765002b2ef 100644 --- a/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp +++ b/rclcpp/src/rclcpp/executors/executor_entities_collection.cpp @@ -232,7 +232,7 @@ ready_executables( if (!waitable) { continue; } - if (!waitable->is_ready(&rcl_wait_set)) { + if (!waitable->is_ready(rcl_wait_set)) { continue; } auto group_info = group_cache(entry.callback_group); diff --git a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp index 85bedcead1..a95bc3797c 100644 --- a/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp +++ b/rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp @@ -45,7 +45,7 @@ ExecutorNotifyWaitable & ExecutorNotifyWaitable::operator=(ExecutorNotifyWaitabl } void -ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) +ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t & wait_set) { std::lock_guard lock(guard_condition_mutex_); for (auto weak_guard_condition : this->notify_guard_conditions_) { @@ -53,8 +53,7 @@ ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) if (!guard_condition) {continue;} rcl_guard_condition_t * cond = &guard_condition->get_rcl_guard_condition(); - - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, cond, NULL); + rcl_ret_t ret = rcl_wait_set_add_guard_condition(&wait_set, cond, NULL); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( @@ -64,13 +63,13 @@ ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set) } bool -ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) +ExecutorNotifyWaitable::is_ready(const rcl_wait_set_t & wait_set) { std::lock_guard lock(guard_condition_mutex_); bool any_ready = false; - for (size_t ii = 0; ii < wait_set->size_of_guard_conditions; ++ii) { - const auto * rcl_guard_condition = wait_set->guard_conditions[ii]; + for (size_t ii = 0; ii < wait_set.size_of_guard_conditions; ++ii) { + const auto * rcl_guard_condition = wait_set.guard_conditions[ii]; if (nullptr == rcl_guard_condition) { continue; @@ -87,7 +86,7 @@ ExecutorNotifyWaitable::is_ready(rcl_wait_set_t * wait_set) } void -ExecutorNotifyWaitable::execute(std::shared_ptr & data) +ExecutorNotifyWaitable::execute(const std::shared_ptr & data) { (void) data; this->execute_callback_(); diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 3348f422f9..831076c61c 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -183,7 +183,7 @@ bool StaticSingleThreadedExecutor::execute_ready_executables( while (auto waitable = wait_result.next_ready_waitable()) { auto entity_iter = collection.waitables.find(waitable.get()); if (entity_iter != collection.waitables.end()) { - auto data = waitable->take_data(); + const auto data = waitable->take_data(); waitable->execute(data); any_ready_executable = true; if (spin_once) {return any_ready_executable;} diff --git a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp index c977c8c904..f7ba6da2df 100644 --- a/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp +++ b/rclcpp/src/rclcpp/experimental/executors/events_executor/events_executor.cpp @@ -339,7 +339,7 @@ EventsExecutor::execute_event(const ExecutorEvent & event) } if (waitable) { for (size_t i = 0; i < event.num_events; i++) { - auto data = waitable->take_data_by_entity_id(event.waitable_data); + const auto data = waitable->take_data_by_entity_id(event.waitable_data); waitable->execute(data); } } diff --git a/rclcpp/src/rclcpp/guard_condition.cpp b/rclcpp/src/rclcpp/guard_condition.cpp index 4f9a85d959..700985f620 100644 --- a/rclcpp/src/rclcpp/guard_condition.cpp +++ b/rclcpp/src/rclcpp/guard_condition.cpp @@ -92,19 +92,19 @@ GuardCondition::exchange_in_use_by_wait_set_state(bool in_use_state) } void -GuardCondition::add_to_wait_set(rcl_wait_set_t * wait_set) +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)) { - if (wait_set != 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; + wait_set_ = &wait_set; } - rcl_ret_t ret = rcl_wait_set_add_guard_condition(wait_set, &this->rcl_guard_condition_, NULL); + 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"); diff --git a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp index 7ecafda36c..f9d19da8c5 100644 --- a/rclcpp/src/rclcpp/subscription_intra_process_base.cpp +++ b/rclcpp/src/rclcpp/subscription_intra_process_base.cpp @@ -18,9 +18,9 @@ using rclcpp::experimental::SubscriptionIntraProcessBase; void -SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t * wait_set) +SubscriptionIntraProcessBase::add_to_wait_set(rcl_wait_set_t & wait_set) { - detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); + detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_); } const char * diff --git a/rclcpp/src/rclcpp/waitable.cpp b/rclcpp/src/rclcpp/waitable.cpp index 6c1284cb22..43b562481c 100644 --- a/rclcpp/src/rclcpp/waitable.cpp +++ b/rclcpp/src/rclcpp/waitable.cpp @@ -86,3 +86,82 @@ Waitable::clear_on_ready_callback() "Custom waitables should override clear_on_ready_callback if they " "want to use it and make sure to call it on the waitable destructor."); } + +void +Waitable::add_to_wait_set(rcl_wait_set_t * wait_set) +{ + this->add_to_wait_set(*wait_set); +} + +void +Waitable::add_to_wait_set(rcl_wait_set_t & wait_set) +{ +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + this->add_to_wait_set(&wait_set); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif +} + +bool +Waitable::is_ready(rcl_wait_set_t * wait_set) +{ + const rcl_wait_set_t & const_wait_set_ref = *wait_set; + return this->is_ready(const_wait_set_ref); +} + +bool +Waitable::is_ready(const rcl_wait_set_t & wait_set) +{ +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + // note this const cast is only required to support a deprecated function + return this->is_ready(&const_cast(wait_set)); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif +} + +void +Waitable::execute(std::shared_ptr & data) +{ + const std::shared_ptr & const_data = data; + this->execute(const_data); +} + +void +Waitable::execute(const std::shared_ptr & data) +{ +#if !defined(_WIN32) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#else // !defined(_WIN32) +# pragma warning(push) +# pragma warning(disable: 4996) +#endif + // note this const cast is only required to support a deprecated function + this->execute(const_cast &>(data)); +// remove warning suppression +#if !defined(_WIN32) +# pragma GCC diagnostic pop +#else // !defined(_WIN32) +# pragma warning(pop) +#endif +} diff --git a/rclcpp/test/rclcpp/executors/test_executors.cpp b/rclcpp/test/rclcpp/executors/test_executors.cpp index 1f4409c7fd..3434f473c6 100644 --- a/rclcpp/test/rclcpp/executors/test_executors.cpp +++ b/rclcpp/test/rclcpp/executors/test_executors.cpp @@ -337,7 +337,7 @@ class TestWaitable : public rclcpp::Waitable TestWaitable() = default; void - add_to_wait_set(rcl_wait_set_t * wait_set) override + add_to_wait_set(rcl_wait_set_t & wait_set) override { if (trigger_count_ > 0) { // Keep the gc triggered until the trigger count is reduced back to zero. @@ -345,7 +345,7 @@ class TestWaitable : public rclcpp::Waitable // executing this waitable, in which case it needs to be re-triggered. gc_.trigger(); } - rclcpp::detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_); + rclcpp::detail::add_guard_condition_to_rcl_wait_set(wait_set, gc_); } void trigger() @@ -355,10 +355,10 @@ class TestWaitable : public rclcpp::Waitable } bool - is_ready(rcl_wait_set_t * wait_set) override + is_ready(const rcl_wait_set_t & wait_set) override { - for (size_t i = 0; i < wait_set->size_of_guard_conditions; ++i) { - auto rcl_guard_condition = wait_set->guard_conditions[i]; + for (size_t i = 0; i < wait_set.size_of_guard_conditions; ++i) { + auto rcl_guard_condition = wait_set.guard_conditions[i]; if (&gc_.get_rcl_guard_condition() == rcl_guard_condition) { return true; } @@ -380,9 +380,8 @@ class TestWaitable : public rclcpp::Waitable } void - execute(std::shared_ptr & data) override + execute(const std::shared_ptr &) override { - (void) data; trigger_count_--; count_++; if (nullptr != on_execute_callback_) { diff --git a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp index 1ea91029f4..1c8c5b3abe 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -230,9 +230,9 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { class TestWaitable : public rclcpp::Waitable { public: - void add_to_wait_set(rcl_wait_set_t *) override {} + void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(rcl_wait_set_t *) override {return true;} + bool is_ready(const rcl_wait_set_t &) override {return true;} std::shared_ptr take_data() override @@ -240,10 +240,7 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } void - execute(std::shared_ptr & data) override - { - (void) data; - } + execute(const std::shared_ptr &) override {} }; TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { @@ -513,11 +510,6 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) { entities_collector_->init(&wait_set, memory_strategy); RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); - EXPECT_THROW( - entities_collector_->add_to_wait_set(nullptr), - std::invalid_argument); - rcl_reset_error(); - EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); } diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp index e559e57cd1..aa34a71af4 100644 --- a/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp +++ b/rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp @@ -28,8 +28,8 @@ class TestWaitable : public rclcpp::Waitable { public: - void add_to_wait_set(rcl_wait_set_t *) override {} - bool is_ready(rcl_wait_set_t *) override {return false;} + void add_to_wait_set(rcl_wait_set_t &) override {} + bool is_ready(const rcl_wait_set_t &) override {return false;} std::shared_ptr take_data() override @@ -37,10 +37,7 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } - void execute(std::shared_ptr & data) override - { - (void) data; - } + void execute(const std::shared_ptr &) override {} }; class TestNodeWaitables : public ::testing::Test diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index b5ff4ff56d..d25b3490c2 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -39,14 +39,14 @@ static bool test_waitable_result = false; class TestWaitable : public rclcpp::Waitable { public: - void add_to_wait_set(rcl_wait_set_t *) override + void add_to_wait_set(rcl_wait_set_t &) override { if (!test_waitable_result) { throw std::runtime_error("TestWaitable add_to_wait_set failed"); } } - bool is_ready(rcl_wait_set_t *) override + bool is_ready(const rcl_wait_set_t &) override { return test_waitable_result; } @@ -57,10 +57,7 @@ class TestWaitable : public rclcpp::Waitable return nullptr; } - void execute(std::shared_ptr & data) override - { - (void) data; - } + void execute(const std::shared_ptr &) override {} }; static bool test_waitable_result2 = false; @@ -82,12 +79,12 @@ class TestWaitable2 : public rclcpp::Waitable EXPECT_EQ(rcl_event_fini(&pub_event_), RCL_RET_OK); } - void add_to_wait_set(rcl_wait_set_t * wait_set) override + void add_to_wait_set(rcl_wait_set_t & wait_set) override { - EXPECT_EQ(rcl_wait_set_add_event(wait_set, &pub_event_, &wait_set_event_index_), RCL_RET_OK); + EXPECT_EQ(rcl_wait_set_add_event(&wait_set, &pub_event_, &wait_set_event_index_), RCL_RET_OK); } - bool is_ready(rcl_wait_set_t *) override + bool is_ready(const rcl_wait_set_t &) override { return test_waitable_result2; } @@ -98,7 +95,7 @@ class TestWaitable2 : public rclcpp::Waitable return nullptr; } - void execute(std::shared_ptr & data) override + void execute(const std::shared_ptr & data) override { (void) data; } diff --git a/rclcpp/test/rclcpp/test_guard_condition.cpp b/rclcpp/test/rclcpp/test_guard_condition.cpp index 90ac9ae2d9..ed54074a4e 100644 --- a/rclcpp/test/rclcpp/test_guard_condition.cpp +++ b/rclcpp/test/rclcpp/test_guard_condition.cpp @@ -105,19 +105,11 @@ TEST_F(TestGuardCondition, add_to_wait_set) { "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)); + 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); + EXPECT_THROW(gc->add_to_wait_set(wait_set_2), std::runtime_error); } } } diff --git a/rclcpp/test/rclcpp/test_memory_strategy.cpp b/rclcpp/test/rclcpp/test_memory_strategy.cpp index c08b84230a..348b7f0143 100644 --- a/rclcpp/test/rclcpp/test_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/test_memory_strategy.cpp @@ -35,10 +35,10 @@ typedef std::map take_data() override {return nullptr;} - void execute(std::shared_ptr & data) override {(void)data;} + void execute(const std::shared_ptr &) override {} }; class TestMemoryStrategy : public ::testing::Test diff --git a/rclcpp/test/rclcpp/test_publisher.cpp b/rclcpp/test/rclcpp/test_publisher.cpp index 949e006997..4284ea48bb 100644 --- a/rclcpp/test/rclcpp/test_publisher.cpp +++ b/rclcpp/test/rclcpp/test_publisher.cpp @@ -526,7 +526,7 @@ TEST_F(TestPublisher, run_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(); + const 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 3803a07d65..ddd6b8db1c 100644 --- a/rclcpp/test/rclcpp/test_qos_event.cpp +++ b/rclcpp/test/rclcpp/test_qos_event.cpp @@ -301,14 +301,14 @@ TEST_F(TestQosEvent, add_to_wait_set) { { rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_add_event, RCL_RET_OK); - EXPECT_NO_THROW(handler.add_to_wait_set(&wait_set)); + EXPECT_NO_THROW(handler.add_to_wait_set(wait_set)); } { rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_wait_set_add_event, RCL_RET_ERROR); - EXPECT_THROW(handler.add_to_wait_set(&wait_set), rclcpp::exceptions::RCLError); + EXPECT_THROW(handler.add_to_wait_set(wait_set), rclcpp::exceptions::RCLError); } } diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp index 9afb97536a..12bd2f884e 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_dynamic_storage.cpp @@ -51,14 +51,14 @@ class TestWaitable : public rclcpp::Waitable TestWaitable() : is_ready_(false) {} - void add_to_wait_set(rcl_wait_set_t *) override {} + void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} void - execute(std::shared_ptr & data) override {(void)data;} + execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp index 470d0de361..55573cc11b 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_static_storage.cpp @@ -50,14 +50,14 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} - void add_to_wait_set(rcl_wait_set_t *) override {} + void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} void - execute(std::shared_ptr & data) override {(void)data;} + execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp index d217286da1..cd44918236 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_storage_policy_common.cpp @@ -50,19 +50,19 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false), add_to_wait_set_(false) {} - void add_to_wait_set(rcl_wait_set_t *) override + void add_to_wait_set(rcl_wait_set_t &) override { if (!add_to_wait_set_) { throw std::runtime_error("waitable unexpectedly failed to be added to wait set"); } } - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} void - execute(std::shared_ptr & data) override {(void)data;} + execute(const std::shared_ptr & data) override {(void)data;} void set_is_ready(bool value) {is_ready_ = value;} diff --git a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp index 347714bbf7..7554f56270 100644 --- a/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp +++ b/rclcpp/test/rclcpp/wait_set_policies/test_thread_safe_synchronization.cpp @@ -50,14 +50,14 @@ class TestWaitable : public rclcpp::Waitable public: TestWaitable() : is_ready_(false) {} - void add_to_wait_set(rcl_wait_set_t *) override {} + void add_to_wait_set(rcl_wait_set_t &) override {} - bool is_ready(rcl_wait_set_t *) override {return is_ready_;} + bool is_ready(const rcl_wait_set_t &) override {return is_ready_;} std::shared_ptr take_data() override {return nullptr;} void - execute(std::shared_ptr & data) override {(void)data;} + execute(const std::shared_ptr &) override {} void set_is_ready(bool value) {is_ready_ = value;} diff --git a/rclcpp_action/include/rclcpp_action/client.hpp b/rclcpp_action/include/rclcpp_action/client.hpp index a9bf2a5008..94cf4aab31 100644 --- a/rclcpp_action/include/rclcpp_action/client.hpp +++ b/rclcpp_action/include/rclcpp_action/client.hpp @@ -122,12 +122,12 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + add_to_wait_set(rcl_wait_set_t & wait_set) override; /// \internal RCLCPP_ACTION_PUBLIC bool - is_ready(rcl_wait_set_t * wait_set) override; + is_ready(const rcl_wait_set_t & wait_set) override; /// \internal RCLCPP_ACTION_PUBLIC @@ -142,7 +142,7 @@ class ClientBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - execute(std::shared_ptr & data) override; + execute(const std::shared_ptr & data) override; /// \internal /// Set a callback to be called when action client entities have an event diff --git a/rclcpp_action/include/rclcpp_action/server.hpp b/rclcpp_action/include/rclcpp_action/server.hpp index 892de5743b..7461de2867 100644 --- a/rclcpp_action/include/rclcpp_action/server.hpp +++ b/rclcpp_action/include/rclcpp_action/server.hpp @@ -119,13 +119,13 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - add_to_wait_set(rcl_wait_set_t * wait_set) override; + add_to_wait_set(rcl_wait_set_t & wait_set) override; /// Return true if any entity belonging to the action server is ready to be executed. /// \internal RCLCPP_ACTION_PUBLIC bool - is_ready(rcl_wait_set_t *) override; + is_ready(const rcl_wait_set_t & wait_set) override; RCLCPP_ACTION_PUBLIC std::shared_ptr @@ -139,7 +139,7 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - execute(std::shared_ptr & data) override; + execute(const std::shared_ptr & data) override; /// \internal /// Set a callback to be called when action server entities have an event @@ -279,19 +279,19 @@ class ServerBase : public rclcpp::Waitable /// \internal RCLCPP_ACTION_PUBLIC void - execute_goal_request_received(std::shared_ptr & data); + execute_goal_request_received(const std::shared_ptr & data); /// Handle a request to cancel goals on the server /// \internal RCLCPP_ACTION_PUBLIC void - execute_cancel_request_received(std::shared_ptr & data); + execute_cancel_request_received(const std::shared_ptr & data); /// Handle a request to get the result of an action /// \internal RCLCPP_ACTION_PUBLIC void - execute_result_request_received(std::shared_ptr & data); + execute_result_request_received(const std::shared_ptr & data); /// Handle a timeout indicating a completed goal should be forgotten by the server /// \internal diff --git a/rclcpp_action/src/client.cpp b/rclcpp_action/src/client.cpp index febd2fd905..fc5b3eeb90 100644 --- a/rclcpp_action/src/client.cpp +++ b/rclcpp_action/src/client.cpp @@ -253,20 +253,21 @@ ClientBase::get_number_of_ready_services() } void -ClientBase::add_to_wait_set(rcl_wait_set_t * wait_set) +ClientBase::add_to_wait_set(rcl_wait_set_t & wait_set) { rcl_ret_t ret = rcl_action_wait_set_add_action_client( - wait_set, pimpl_->client_handle.get(), nullptr, nullptr); + &wait_set, pimpl_->client_handle.get(), nullptr, nullptr); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "ClientBase::add_to_wait_set() failed"); } } bool -ClientBase::is_ready(rcl_wait_set_t * wait_set) +ClientBase::is_ready(const rcl_wait_set_t & wait_set) { rcl_ret_t ret = rcl_action_client_wait_set_get_entities_ready( - wait_set, pimpl_->client_handle.get(), + &wait_set, + pimpl_->client_handle.get(), &pimpl_->is_feedback_ready, &pimpl_->is_status_ready, &pimpl_->is_goal_response_ready, @@ -619,7 +620,7 @@ ClientBase::take_data_by_entity_id(size_t id) } void -ClientBase::execute(std::shared_ptr & data) +ClientBase::execute(const std::shared_ptr & data) { if (!data) { throw std::runtime_error("'data' is empty"); diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 780b2ba20c..565aaa4f3b 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -165,18 +165,18 @@ ServerBase::get_number_of_ready_guard_conditions() } void -ServerBase::add_to_wait_set(rcl_wait_set_t * wait_set) +ServerBase::add_to_wait_set(rcl_wait_set_t & wait_set) { std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); rcl_ret_t ret = rcl_action_wait_set_add_action_server( - wait_set, pimpl_->action_server_.get(), NULL); + &wait_set, pimpl_->action_server_.get(), NULL); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "ServerBase::add_to_wait_set() failed"); } } bool -ServerBase::is_ready(rcl_wait_set_t * wait_set) +ServerBase::is_ready(const rcl_wait_set_t & wait_set) { bool goal_request_ready; bool cancel_request_ready; @@ -186,7 +186,7 @@ ServerBase::is_ready(rcl_wait_set_t * wait_set) { std::lock_guard lock(pimpl_->action_server_reentrant_mutex_); ret = rcl_action_server_wait_set_get_entities_ready( - wait_set, + &wait_set, pimpl_->action_server_.get(), &goal_request_ready, &cancel_request_ready, @@ -287,7 +287,7 @@ ServerBase::take_data_by_entity_id(size_t id) } void -ServerBase::execute(std::shared_ptr & data) +ServerBase::execute(const std::shared_ptr & data) { if (!data && !pimpl_->goal_expired_.load()) { throw std::runtime_error("'data' is empty"); @@ -307,7 +307,7 @@ ServerBase::execute(std::shared_ptr & data) } void -ServerBase::execute_goal_request_received(std::shared_ptr & data) +ServerBase::execute_goal_request_received(const std::shared_ptr & data) { auto shared_ptr = std::static_pointer_cast >>(data); @@ -405,11 +405,10 @@ ServerBase::execute_goal_request_received(std::shared_ptr & data) // Tell user to start executing action call_goal_accepted_callback(handle, uuid, message); } - data.reset(); } void -ServerBase::execute_cancel_request_received(std::shared_ptr & data) +ServerBase::execute_cancel_request_received(const std::shared_ptr & data) { auto shared_ptr = std::static_pointer_cast , @@ -504,11 +503,10 @@ ServerBase::execute_cancel_request_received(std::shared_ptr & data) if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret); } - data.reset(); } void -ServerBase::execute_result_request_received(std::shared_ptr & data) +ServerBase::execute_result_request_received(const std::shared_ptr & data) { auto shared_ptr = std::static_pointer_cast , rmw_request_id_t>>(data); @@ -568,7 +566,6 @@ ServerBase::execute_result_request_received(std::shared_ptr & data) rclcpp::exceptions::throw_from_rcl_error(rcl_ret); } } - data.reset(); } void diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index d5be45840b..08093cb873 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -397,13 +397,13 @@ TEST_F(TestClient, is_ready) { ASSERT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 10, 10, 10, 10, 10, 10, rcl_context, allocator)); - ASSERT_NO_THROW(action_client->add_to_wait_set(&wait_set)); - EXPECT_TRUE(action_client->is_ready(&wait_set)); + ASSERT_NO_THROW(action_client->add_to_wait_set(wait_set)); + EXPECT_TRUE(action_client->is_ready(wait_set)); { auto mock = mocking_utils::patch_and_return( "lib:rclcpp_action", rcl_action_client_wait_set_get_entities_ready, RCL_RET_ERROR); - EXPECT_THROW(action_client->is_ready(&wait_set), rclcpp::exceptions::RCLError); + EXPECT_THROW(action_client->is_ready(wait_set), rclcpp::exceptions::RCLError); } client_node.reset(); // Drop node before action client } diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 8687f90fbe..9ffa31797f 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -1107,12 +1107,12 @@ TEST_F(TestGoalRequestServer, is_ready_rcl_error) { { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); }); - EXPECT_NO_THROW(action_server_->add_to_wait_set(&wait_set)); + EXPECT_NO_THROW(action_server_->add_to_wait_set(wait_set)); - EXPECT_TRUE(action_server_->is_ready(&wait_set)); + EXPECT_TRUE(action_server_->is_ready(wait_set)); auto mock = mocking_utils::patch_and_return( "lib:rclcpp_action", rcl_action_server_wait_set_get_entities_ready, RCL_RET_ERROR); - EXPECT_THROW(action_server_->is_ready(&wait_set), rclcpp::exceptions::RCLError); + EXPECT_THROW(action_server_->is_ready(wait_set), rclcpp::exceptions::RCLError); } TEST_F(TestGoalRequestServer, execute_goal_request_received_take_goal_request_errors)