From 85a15defa922e2da670b503e18c70c12481d8fbc Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Thu, 22 Jul 2021 15:58:08 -0400 Subject: [PATCH 1/4] Use rcpputils/scope_exit.hpp and remove rclcpp/scope_exit.hpp Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/executor.hpp | 4 +- rclcpp/include/rclcpp/rclcpp.hpp | 1 - rclcpp/include/rclcpp/scope_exit.hpp | 52 ------------------- rclcpp/include/rclcpp/wait_set_template.hpp | 4 +- .../detail/resolve_parameter_overrides.cpp | 4 +- rclcpp/src/rclcpp/executor.cpp | 6 +-- .../executors/multi_threaded_executor.cpp | 5 +- .../executors/single_threaded_executor.cpp | 5 +- .../static_single_threaded_executor.cpp | 6 +-- .../rclcpp/expand_topic_or_service_name.cpp | 1 - .../node_interfaces/node_parameters.cpp | 1 - rclcpp/test/benchmark/benchmark_executor.cpp | 6 +-- ...est_static_executor_entities_collector.cpp | 44 ++++++++-------- .../test_allocator_memory_strategy.cpp | 8 +-- rclcpp/test/rclcpp/test_node.cpp | 23 +++++--- rclcpp/test/rclcpp/test_utilities.cpp | 1 - rclcpp_action/CMakeLists.txt | 3 ++ rclcpp_action/package.xml | 1 + rclcpp_action/src/server.cpp | 7 +-- rclcpp_action/test/test_server.cpp | 4 +- rclcpp_lifecycle/CMakeLists.txt | 1 + rclcpp_lifecycle/package.xml | 1 + .../test/test_lifecycle_service_client.cpp | 5 +- 23 files changed, 79 insertions(+), 114 deletions(-) delete mode 100644 rclcpp/include/rclcpp/scope_exit.hpp diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp index 13ed108348..ca00f1309e 100644 --- a/rclcpp/include/rclcpp/executor.hpp +++ b/rclcpp/include/rclcpp/executor.hpp @@ -29,6 +29,7 @@ #include "rcl/guard_condition.h" #include "rcl/wait.h" +#include "rcpputils/scope_exit.hpp" #include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" @@ -40,7 +41,6 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/utilities.hpp" #include "rclcpp/visibility_control.hpp" -#include "rclcpp/scope_exit.hpp" namespace rclcpp { @@ -354,7 +354,7 @@ class Executor if (spinning.exchange(true)) { throw std::runtime_error("spin_until_future_complete() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); while (rclcpp::ok(this->context_) && spinning.load()) { // Do one item of work. spin_once_impl(timeout_left); diff --git a/rclcpp/include/rclcpp/rclcpp.hpp b/rclcpp/include/rclcpp/rclcpp.hpp index 7f0498fe49..f1d751ff3f 100644 --- a/rclcpp/include/rclcpp/rclcpp.hpp +++ b/rclcpp/include/rclcpp/rclcpp.hpp @@ -140,7 +140,6 @@ * - rclcpp/duration.hpp * - rclcpp/function_traits.hpp * - rclcpp/macros.hpp - * - rclcpp/scope_exit.hpp * - rclcpp/time.hpp * - rclcpp/utilities.hpp * - rclcpp/typesupport_helpers.hpp diff --git a/rclcpp/include/rclcpp/scope_exit.hpp b/rclcpp/include/rclcpp/scope_exit.hpp deleted file mode 100644 index 7865de5e48..0000000000 --- a/rclcpp/include/rclcpp/scope_exit.hpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2015 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. - -// Based on: http://the-witness.net/news/2012/11/scopeexit-in-c11/ -// But I changed the lambda to include by reference rather than value, see: -// http://the-witness.net/news/2012/11/scopeexit-in-c11/comment-page-1/#comment-86873 - -#ifndef RCLCPP__SCOPE_EXIT_HPP_ -#define RCLCPP__SCOPE_EXIT_HPP_ - -#include - -#include "rclcpp/macros.hpp" - -namespace rclcpp -{ - -template -struct ScopeExit -{ - explicit ScopeExit(Callable callable) - : callable_(callable) {} - ~ScopeExit() {callable_();} - -private: - Callable callable_; -}; - -template -ScopeExit -make_scope_exit(Callable callable) -{ - return ScopeExit(callable); -} - -} // namespace rclcpp - -#define RCLCPP_SCOPE_EXIT(code) \ - auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code;}) - -#endif // RCLCPP__SCOPE_EXIT_HPP_ diff --git a/rclcpp/include/rclcpp/wait_set_template.hpp b/rclcpp/include/rclcpp/wait_set_template.hpp index 39c7dd3d4e..3adee41164 100644 --- a/rclcpp/include/rclcpp/wait_set_template.hpp +++ b/rclcpp/include/rclcpp/wait_set_template.hpp @@ -20,13 +20,13 @@ #include #include "rcl/wait.h" +#include "rcpputils/scope_exit.hpp" #include "rclcpp/client.hpp" #include "rclcpp/context.hpp" #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/macros.hpp" -#include "rclcpp/scope_exit.hpp" #include "rclcpp/service.hpp" #include "rclcpp/subscription_base.hpp" #include "rclcpp/subscription_wait_set_mask.hpp" @@ -657,7 +657,7 @@ class WaitSetTemplate final : private SynchronizationPolicy, private StoragePoli // ensure the ownership of the entities in the wait set is shared for the duration of wait this->storage_acquire_ownerships(); - RCLCPP_SCOPE_EXIT({this->storage_release_ownerships();}); + RCPPUTILS_SCOPE_EXIT({this->storage_release_ownerships();}); // this method comes from the SynchronizationPolicy return this->template sync_wait>( diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp index bb8b61dca6..15f567f7c5 100644 --- a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp +++ b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp @@ -19,8 +19,8 @@ #include #include "rcl_yaml_param_parser/parser.h" +#include "rcpputils/scope_exit.hpp" -#include "rclcpp/scope_exit.hpp" #include "rclcpp/parameter_map.hpp" std::map @@ -47,7 +47,7 @@ rclcpp::detail::resolve_parameter_overrides( rclcpp::exceptions::throw_from_rcl_error(ret); } if (params) { - auto cleanup_params = make_scope_exit( + auto cleanup_params = rcpputils::make_scope_exit( [params]() { rcl_yaml_node_struct_fini(params); }); diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index a69c89d735..b247457521 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -22,13 +22,13 @@ #include "rcl/allocator.h" #include "rcl/error_handling.h" +#include "rcpputils/scope_exit.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/executor.hpp" #include "rclcpp/guard_condition.hpp" #include "rclcpp/memory_strategy.hpp" #include "rclcpp/node.hpp" -#include "rclcpp/scope_exit.hpp" #include "rclcpp/utilities.hpp" #include "rcutils/logging_macros.h" @@ -450,7 +450,7 @@ Executor::spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive) if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); bool work_available = false; while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { AnyExecutable any_exec; @@ -484,7 +484,7 @@ Executor::spin_once(std::chrono::nanoseconds timeout) if (spinning.exchange(true)) { throw std::runtime_error("spin_once() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); spin_once_impl(timeout); } diff --git a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp index a9b01b0c34..e2b95d463a 100644 --- a/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/multi_threaded_executor.cpp @@ -19,8 +19,9 @@ #include #include +#include "rcpputils/scope_exit.hpp" + #include "rclcpp/utilities.hpp" -#include "rclcpp/scope_exit.hpp" using rclcpp::detail::MutexTwoPriorities; using rclcpp::executors::MultiThreadedExecutor; @@ -48,7 +49,7 @@ MultiThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); std::vector threads; size_t thread_id = 0; { diff --git a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp index 360574f7a1..e7f311c147 100644 --- a/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/single_threaded_executor.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "rcpputils/scope_exit.hpp" + #include "rclcpp/executors/single_threaded_executor.hpp" #include "rclcpp/any_executable.hpp" -#include "rclcpp/scope_exit.hpp" using rclcpp::executors::SingleThreadedExecutor; @@ -29,7 +30,7 @@ SingleThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); while (rclcpp::ok(this->context_) && spinning.load()) { rclcpp::AnyExecutable any_executable; if (get_next_executable(any_executable)) { diff --git a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp index 5402d32460..bc27bdf464 100644 --- a/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp +++ b/rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp @@ -18,7 +18,7 @@ #include #include -#include "rclcpp/scope_exit.hpp" +#include "rcpputils/scope_exit.hpp" using rclcpp::executors::StaticSingleThreadedExecutor; using rclcpp::experimental::ExecutableList; @@ -43,7 +43,7 @@ StaticSingleThreadedExecutor::spin() if (spinning.exchange(true)) { throw std::runtime_error("spin() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); // Set memory_strategy_ and exec_list_ based on weak_nodes_ // Prepare wait_set_ based on memory_strategy_ @@ -100,7 +100,7 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati if (spinning.exchange(true)) { throw std::runtime_error("spin_some() called while already spinning"); } - RCLCPP_SCOPE_EXIT(this->spinning.store(false); ); + RCPPUTILS_SCOPE_EXIT(this->spinning.store(false); ); while (rclcpp::ok(context_) && spinning.load() && max_duration_not_elapsed()) { // Get executables that are ready now diff --git a/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp b/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp index b6976e21c9..79aecdad42 100644 --- a/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp +++ b/rclcpp/src/rclcpp/expand_topic_or_service_name.cpp @@ -19,7 +19,6 @@ #include "rcl/expand_topic_name.h" #include "rcl/validate_topic_name.h" #include "rclcpp/exceptions.hpp" -#include "rclcpp/scope_exit.hpp" #include "rcutils/logging_macros.h" #include "rcutils/types/string_map.h" #include "rmw/error_handling.h" diff --git a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp index 3d91943bc5..92d42bd8e7 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_parameters.cpp @@ -32,7 +32,6 @@ #include "rcl_interfaces/srv/list_parameters.hpp" #include "rclcpp/create_publisher.hpp" #include "rclcpp/parameter_map.hpp" -#include "rclcpp/scope_exit.hpp" #include "rcutils/logging_macros.h" #include "rmw/qos_profiles.h" diff --git a/rclcpp/test/benchmark/benchmark_executor.cpp b/rclcpp/test/benchmark/benchmark_executor.cpp index c1fb3cf52c..85815b3b50 100644 --- a/rclcpp/test/benchmark/benchmark_executor.cpp +++ b/rclcpp/test/benchmark/benchmark_executor.cpp @@ -20,7 +20,7 @@ #include "performance_test_fixture/performance_test_fixture.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp/scope_exit.hpp" +#include "rcpputils/scope_exit.hpp" #include "test_msgs/msg/empty.hpp" using namespace std::chrono_literals; @@ -366,7 +366,7 @@ BENCHMARK_F( if (ret != RCL_RET_OK) { st.SkipWithError(rcutils_get_error_string().str); } - RCLCPP_SCOPE_EXIT( + RCPPUTILS_SCOPE_EXIT( { rcl_ret_t ret = rcl_wait_set_fini(&wait_set); if (ret != RCL_RET_OK) { @@ -379,7 +379,7 @@ BENCHMARK_F( rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); reset_heap_counters(); 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 fba322f9e2..0ad48d3a2d 100644 --- a/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp +++ b/rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp @@ -20,6 +20,8 @@ #include "rclcpp/rclcpp.hpp" +#include "rcpputils/scope_exit.hpp" + #include "test_msgs/msg/empty.hpp" #include "test_msgs/srv/empty.hpp" @@ -138,7 +140,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, init_bad_arguments) { EXPECT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); - RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); rclcpp::GuardCondition guard_condition(shared_context); rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); @@ -163,14 +165,14 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_basic_node) { EXPECT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); - RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ( expected_number_of_entities->subscriptions, entities_collector_->get_number_of_subscriptions()); @@ -210,7 +212,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { EXPECT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); - RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); @@ -218,7 +220,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) { // Expect weak_node pointers to be cleaned up and used entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ(0u, entities_collector_->get_number_of_subscriptions()); EXPECT_EQ(0u, entities_collector_->get_number_of_timers()); EXPECT_EQ(0u, entities_collector_->get_number_of_services()); @@ -281,7 +283,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { EXPECT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); - RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); @@ -289,7 +291,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_with_entities) { rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); EXPECT_EQ( 1u + expected_number_of_entities->subscriptions, @@ -361,14 +363,14 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_clear_ EXPECT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); - RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); { auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_clear, RCL_RET_ERROR); @@ -392,14 +394,14 @@ TEST_F(TestStaticExecutorEntitiesCollector, prepare_wait_set_rcl_wait_set_resize EXPECT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); - RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); { auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait_set_resize, RCL_RET_ERROR); @@ -430,14 +432,14 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_rcl_wait_failed) { EXPECT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); - RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); { auto mock = mocking_utils::patch_and_return("lib:rclcpp", rcl_wait, RCL_RET_ERROR); @@ -478,7 +480,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait EXPECT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); - RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); @@ -486,7 +488,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, refresh_wait_set_add_handles_to_wait rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); { auto mock = mocking_utils::patch_and_return( @@ -511,14 +513,14 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_to_wait_set_nullptr) { EXPECT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); - RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); RCLCPP_EXPECT_THROW_EQ( entities_collector_->add_to_wait_set(nullptr), @@ -539,7 +541,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) EXPECT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); - RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); @@ -554,7 +556,7 @@ TEST_F(TestStaticExecutorEntitiesCollector, fill_memory_strategy_invalid_group) cb_group.reset(); entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); ASSERT_EQ(entities_collector_->get_all_callback_groups().size(), 1u); EXPECT_TRUE(entities_collector_->remove_node(node->get_node_base_interface())); @@ -617,14 +619,14 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 100, 100, 100, 100, 100, 100, context, allocator)); - RCLCPP_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); + RCPPUTILS_SCOPE_EXIT({EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set));}); auto memory_strategy = rclcpp::memory_strategies::create_default_strategy(); rclcpp::GuardCondition guard_condition(shared_context); rcl_guard_condition_t rcl_guard_condition = guard_condition.get_rcl_guard_condition(); entities_collector_->init(&wait_set, memory_strategy, &rcl_guard_condition); - RCLCPP_SCOPE_EXIT(entities_collector_->fini()); + RCPPUTILS_SCOPE_EXIT(entities_collector_->fini()); cb_group->get_associated_with_executor_atomic().exchange(false); std::shared_ptr data = entities_collector_->take_data(); diff --git a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp index f57171ec8e..8899fb5470 100644 --- a/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp +++ b/rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp @@ -21,8 +21,8 @@ #include "gtest/gtest.h" -#include "rclcpp/scope_exit.hpp" #include "rclcpp/strategies/allocator_memory_strategy.hpp" +#include "rcpputils/scope_exit.hpp" #include "test_msgs/msg/empty.hpp" #include "test_msgs/srv/empty.hpp" @@ -268,7 +268,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test "Calling rcl_wait_set_init() with expected sufficient capacities failed"; } - RCLCPP_SCOPE_EXIT( + RCPPUTILS_SCOPE_EXIT( { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); }); @@ -296,7 +296,7 @@ class TestAllocatorMemoryStrategy : public ::testing::Test "Calling rcl_wait_set_init() with expected insufficient capacities failed"; } - RCLCPP_SCOPE_EXIT( + RCPPUTILS_SCOPE_EXIT( { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set_no_capacity)); }); @@ -594,7 +594,7 @@ TEST_F(TestAllocatorMemoryStrategy, add_handles_to_wait_set_guard_condition) { EXPECT_EQ( RCL_RET_OK, rcl_guard_condition_init(&guard_condition, rcl_context, guard_condition_options)); - RCLCPP_SCOPE_EXIT( + RCPPUTILS_SCOPE_EXIT( { EXPECT_EQ(RCL_RET_OK, rcl_guard_condition_fini(&guard_condition)); }); diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index 1ac009ebc8..1fbe1799ce 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -25,10 +25,10 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/node.hpp" -#include "rclcpp/scope_exit.hpp" #include "rclcpp/rclcpp.hpp" #include "rcpputils/filesystem_helper.hpp" +#include "rcpputils/scope_exit.hpp" #include "rmw/validate_namespace.h" @@ -410,7 +410,8 @@ TEST_F(TestNode, declare_parameter_with_no_initial_values) { return result; }; auto handler = node->add_on_set_parameters_callback(on_set_parameters); - RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset EXPECT_THROW( {node->declare_parameter(name, "not an int");}, rclcpp::exceptions::InvalidParameterValueException); @@ -657,7 +658,8 @@ TEST_F(TestNode, declare_parameter_with_overrides) { return result; }; auto handler = node->add_on_set_parameters_callback(on_set_parameters); - RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset EXPECT_THROW( {node->declare_parameter(name, 43);}, rclcpp::exceptions::InvalidParameterValueException); @@ -784,7 +786,8 @@ TEST_F(TestNode, declare_parameters_with_no_initial_values) { return result; }; auto handler = node->add_on_set_parameters_callback(on_set_parameters); - RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset EXPECT_THROW( {node->declare_parameters("", {{name, "not an int"}});}, rclcpp::exceptions::InvalidParameterValueException); @@ -1012,7 +1015,8 @@ TEST_F(TestNode, set_parameter_undeclared_parameters_not_allowed) { return result; }; auto handler = node->add_on_set_parameters_callback(on_set_parameters); - RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset EXPECT_FALSE(node->set_parameter(rclcpp::Parameter(name, 43)).successful); } @@ -1520,7 +1524,8 @@ TEST_F(TestNode, set_parameters_undeclared_parameters_not_allowed) { return result; }; auto handler = node->add_on_set_parameters_callback(on_set_parameters); - RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset auto rets = node->set_parameters( { @@ -1712,7 +1717,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_not_allowed) { return result; }; auto handler = node->add_on_set_parameters_callback(on_set_parameters); - RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset auto ret = node->set_parameters_atomically( { @@ -1838,7 +1844,8 @@ TEST_F(TestNode, set_parameters_atomically_undeclared_parameters_allowed) { return result; }; auto handler = node->add_on_set_parameters_callback(on_set_parameters); - RCLCPP_SCOPE_EXIT({node->remove_on_set_parameters_callback(handler.get());}); // always reset + RCPPUTILS_SCOPE_EXIT( + {node->remove_on_set_parameters_callback(handler.get());}); // always reset auto ret = node->set_parameters_atomically( { diff --git a/rclcpp/test/rclcpp/test_utilities.cpp b/rclcpp/test/rclcpp/test_utilities.cpp index f9d6b5d8e3..c57fcb9fc3 100644 --- a/rclcpp/test/rclcpp/test_utilities.cpp +++ b/rclcpp/test/rclcpp/test_utilities.cpp @@ -24,7 +24,6 @@ #include "rclcpp/contexts/default_context.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/utilities.hpp" -#include "rclcpp/scope_exit.hpp" #include "../mocking_utils/patch.hpp" #include "../utils/rclcpp_gtest_macros.hpp" diff --git a/rclcpp_action/CMakeLists.txt b/rclcpp_action/CMakeLists.txt index a5b100e5d6..e9bd327643 100644 --- a/rclcpp_action/CMakeLists.txt +++ b/rclcpp_action/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(ament_cmake_ros REQUIRED) find_package(action_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(rcl_action REQUIRED) +find_package(rcpputils REQUIRED) find_package(rosidl_runtime_c REQUIRED) # Default to C++14 @@ -39,6 +40,7 @@ ament_target_dependencies(${PROJECT_NAME} "action_msgs" "rcl_action" "rclcpp" + "rcpputils" "rosidl_runtime_c" ) @@ -94,6 +96,7 @@ if(BUILD_TESTING) ament_add_gtest(test_server test/test_server.cpp TIMEOUT 180) if(TARGET test_server) ament_target_dependencies(test_server + "rcpputils" "rcutils" "test_msgs" ) diff --git a/rclcpp_action/package.xml b/rclcpp_action/package.xml index 13db81bb21..a8a2488459 100644 --- a/rclcpp_action/package.xml +++ b/rclcpp_action/package.xml @@ -19,6 +19,7 @@ action_msgs rclcpp rcl_action + rcpputils ament_cmake diff --git a/rclcpp_action/src/server.cpp b/rclcpp_action/src/server.cpp index 509e152384..d1477300f1 100644 --- a/rclcpp_action/src/server.cpp +++ b/rclcpp_action/src/server.cpp @@ -15,10 +15,11 @@ #include #include +#include + #include #include #include -#include #include #include @@ -416,7 +417,7 @@ ServerBase::execute_cancel_request_received(std::shared_ptr & data) rclcpp::exceptions::throw_from_rcl_error(ret); } - RCLCPP_SCOPE_EXIT( + RCPPUTILS_SCOPE_EXIT( { ret = rcl_action_cancel_response_fini(&cancel_response); if (RCL_RET_OK != ret) { @@ -581,7 +582,7 @@ ServerBase::publish_status() rclcpp::exceptions::throw_from_rcl_error(ret); } - RCLCPP_SCOPE_EXIT( + RCPPUTILS_SCOPE_EXIT( { ret = rcl_action_goal_status_array_fini(&c_status_array); if (RCL_RET_OK != ret) { diff --git a/rclcpp_action/test/test_server.cpp b/rclcpp_action/test/test_server.cpp index 1a0b074ba3..6f8d874359 100644 --- a/rclcpp_action/test/test_server.cpp +++ b/rclcpp_action/test/test_server.cpp @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include #include @@ -1082,7 +1082,7 @@ TEST_F(TestGoalRequestServer, is_ready_rcl_error) { ASSERT_EQ( RCL_RET_OK, rcl_wait_set_init(&wait_set, 10, 10, 10, 10, 10, 10, rcl_context, allocator)); - RCLCPP_SCOPE_EXIT( + RCPPUTILS_SCOPE_EXIT( { EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); }); diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 627dc2061c..e73a5a3ffd 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -103,6 +103,7 @@ if(BUILD_TESTING) ament_target_dependencies(test_lifecycle_service_client "rcl_lifecycle" "rclcpp" + "rcpputils" "rcutils" ) target_link_libraries(test_lifecycle_service_client ${PROJECT_NAME} mimick) diff --git a/rclcpp_lifecycle/package.xml b/rclcpp_lifecycle/package.xml index 6a0dbac12a..7fffdc8bdb 100644 --- a/rclcpp_lifecycle/package.xml +++ b/rclcpp_lifecycle/package.xml @@ -29,6 +29,7 @@ ament_lint_common mimick_vendor performance_test_fixture + rcpputils rcutils test_msgs diff --git a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp index 109b1448a3..d7188ca350 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp @@ -37,6 +37,8 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rcpputils/scope_exit.hpp" + #include "./mocking_utils/patch.hpp" using namespace std::chrono_literals; @@ -395,7 +397,8 @@ TEST_F(TestLifecycleServiceClient, declare_parameter_with_no_initial_values) }; auto handler = node1->add_on_set_parameters_callback(on_set_parameters); - RCLCPP_SCOPE_EXIT({node1->remove_on_set_parameters_callback(handler.get());}); // always reset + RCPPUTILS_SCOPE_EXIT( + {node1->remove_on_set_parameters_callback(handler.get());}); // always reset } TEST_F(TestLifecycleServiceClient, wait_for_graph_change) From 072dabd4700b6ba22411b64a56bb2142d3939312 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Thu, 22 Jul 2021 18:28:23 -0400 Subject: [PATCH 2/4] Remove RCLCPP_STRING_JOIN() since it's unused & also provided by rcutils Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/macros.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/rclcpp/include/rclcpp/macros.hpp b/rclcpp/include/rclcpp/macros.hpp index d13c5d21b9..611efc5b5b 100644 --- a/rclcpp/include/rclcpp/macros.hpp +++ b/rclcpp/include/rclcpp/macros.hpp @@ -108,7 +108,4 @@ __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ __RCLCPP_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) -#define RCLCPP_STRING_JOIN(arg1, arg2) RCLCPP_DO_STRING_JOIN(arg1, arg2) -#define RCLCPP_DO_STRING_JOIN(arg1, arg2) arg1 ## arg2 - #endif // RCLCPP__MACROS_HPP_ From 92da89e03244c2a99cf3d18748533e26d3408800 Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Thu, 22 Jul 2021 18:28:23 -0400 Subject: [PATCH 3/4] Revert "Remove RCLCPP_STRING_JOIN() since it's unused & also provided by rcutils" This reverts commit 072dabd4700b6ba22411b64a56bb2142d3939312. Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/macros.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp/include/rclcpp/macros.hpp b/rclcpp/include/rclcpp/macros.hpp index 611efc5b5b..d13c5d21b9 100644 --- a/rclcpp/include/rclcpp/macros.hpp +++ b/rclcpp/include/rclcpp/macros.hpp @@ -108,4 +108,7 @@ __RCLCPP_UNIQUE_PTR_ALIAS(__VA_ARGS__) \ __RCLCPP_MAKE_UNIQUE_DEFINITION(__VA_ARGS__) +#define RCLCPP_STRING_JOIN(arg1, arg2) RCLCPP_DO_STRING_JOIN(arg1, arg2) +#define RCLCPP_DO_STRING_JOIN(arg1, arg2) arg1 ## arg2 + #endif // RCLCPP__MACROS_HPP_ From 68adbafe7566d76729ce3f405648e59ee8b68caf Mon Sep 17 00:00:00 2001 From: Christophe Bedard Date: Fri, 23 Jul 2021 09:40:08 -0400 Subject: [PATCH 4/4] Re-add but deprecate rclcpp/scope_exit.hpp Signed-off-by: Christophe Bedard --- rclcpp/include/rclcpp/scope_exit.hpp | 56 ++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 rclcpp/include/rclcpp/scope_exit.hpp diff --git a/rclcpp/include/rclcpp/scope_exit.hpp b/rclcpp/include/rclcpp/scope_exit.hpp new file mode 100644 index 0000000000..8b8e3ea49b --- /dev/null +++ b/rclcpp/include/rclcpp/scope_exit.hpp @@ -0,0 +1,56 @@ +// Copyright 2015 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. + +// Based on: http://the-witness.net/news/2012/11/scopeexit-in-c11/ +// But I changed the lambda to include by reference rather than value, see: +// http://the-witness.net/news/2012/11/scopeexit-in-c11/comment-page-1/#comment-86873 + +#ifndef RCLCPP__SCOPE_EXIT_HPP_ +#define RCLCPP__SCOPE_EXIT_HPP_ + +// TODO(christophebedard) remove this header completely in I-turtle + +#warning rclcpp/scope_exit.hpp has been deprecated, please use rcpputils/scope_exit.hpp instead + +#include + +#include "rclcpp/macros.hpp" + +namespace rclcpp +{ + +template +struct ScopeExit +{ + explicit ScopeExit(Callable callable) + : callable_(callable) {} + ~ScopeExit() {callable_();} + +private: + Callable callable_; +}; + +template +ScopeExit +make_scope_exit(Callable callable) +{ + return ScopeExit(callable); +} + +} // namespace rclcpp + +#define RCLCPP_SCOPE_EXIT(code) \ + auto RCLCPP_STRING_JOIN(scope_exit_, __LINE__) = rclcpp::make_scope_exit([&]() {code;}) + +#endif // RCLCPP__SCOPE_EXIT_HPP_